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

%% Initializtion
% load your trajectory file
load test_lab3.mat

% convert to degrees (if necessary!)
q1 = q1*180/pi; % angles for theta1
q2 = q2*180/pi; % angles for theta2
q3 = q3*180/pi; % angles for theta2

% number of points in your trajectory
nTrajPts = length(q1);
% 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 = 20;

scale_factor = nTrajPts/nSec;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Switch to use feedforward control
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
ff_switch=0; % 1: FF ON,  0: FF OFF
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

% proportional gain
KpA = ? % continuous time model

% integral 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

% proportional gain
KpB = ? % continuous time model

% integral 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;


%% Create Controller for Motor C

% proportional gain
KpC = ? % continuous time model

% integral controller gain
KiC = ?; % continuous time model

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

%% create controllers for simulink model

controllerA = CA_discr;
controllerB = CB_discr;
controllerC = CC_discr;

