%% Model paramters

%mtot = .719;                   %weight of entire segway
g = 9.81;						% gravity acceleration [m/sec^2]
m = 0.028; 						% wheel weight [kg]
R = 0.031;						% wheel radius [m]
Jw = m * R^2 / 2;				% wheel inertia moment [kgm^2]
M = 0.603;						% body weight [kg]
W = 0.0643;                     % body width [m]
D = 0.04;						% body depth [m]
H = 0.144;						% body height [m]
L = H / 2;						% distance of the center of mass from the wheel axle [m]
Jpsi = M * L^2 / 3;				% body pitch inertia moment [kgm^2]
Jphi = M * (W^2 + D^2) / 12;	% body yaw inertia moment [kgm^2]
fm = 0.0022;					% friction coefficient between body & DC motor
fw = 0;							% friction coefficient between wheel & floor

% DC Motor Parameters			
Jm = 1e-5;						% DC motor inertia moment [kgm^2]
Rm = 6.69;						% DC motor resistance [ƒ¶]
Kb = 0.468;						% DC motor back EMF constant [Vsec/rad]
Kt = 0.317;						% DC motor torque constant [Nm/A]
n = 1;							% gear ratio

% 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

%% **Filtering**

%Complimentary filters
Ga = tf([1], [2,1]);    %generate accel filter
A=Ha*Ga;
Gg = minreal( (1 - Ha*Ga)/(tf([1,0],[1])*Hg) ); %solve for gyro filter
B = tf([1,0],[1])*Gg;

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=10;
VF = tf([f],[1, f]);
VFd = c2d(VF, Ts, 'tustin');
VoltageFilter = tf(VFd.num, VFd.den,-1);
LowPass= VoltageFilter; %derivative filter
