% script to set gains for your controller and intialize your transfer
% functions for simulink model

%% Initializtion
% load your trajectory file
load test1

% convert to degrees (if necessary!)
a1 = a1*180/pi; % angles for theta1
a2 = a2*180/pi; % angles for theta2

% number of points in your trajectory
nTrajPts = length(a1);
% This is important because it effects how fast you will be playing your
% trajectory in the nxt model

%% Changing Length of time Trajectory is played
% How many seconds you want your entire trajectory to be played in
nSec = 1/2;

scale_factor = nTrajPts/nSec;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Switch to use feedforward control
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
ff_switch=1;
FF_A = 40;      % Feedforward Value (Power Level)
FF_B=40;        % Feedforward Value

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Proportional Integrator Constants (Tuning of Controller Done Here)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

fs = 1e3; % Sampling time of nxt block DO NOT CHANGE!!!!
ts = 1/fs;

s = tf('s');

%% Create Controller for Motor A (theta1)

%integral gain
KpA = ? % continuous time model

% proportional controller gain
KiA = ?; % continuous time model

% summed total of proportional and integral controller
CA_cont = KpA + KiA/s;
CA_discr = c2d(CA_cont, ts, 'tustin');

%% Create Controller for Motor B (theta2)

%integral gain
KpB = ? % continuous time model

% proportional controller gain
KiB = ?; % continuous time model

% summed total of proportional and integral controller
CB_cont = KpB + KiB/s;
CB_discr = c2d(CB_cont, ts, 'tustin');

controllerA = CA_discr;
controllerB = CB_discr;

