Add Disturbances and Reference initialization

src/initializeController.m

@@ -7,7 +7,7 @@ function [controller] = initializeController(args)
 %    - args - Can have the following fields:
 
 arguments
-  args.type   char   {mustBeMember(args.type, {'open-loop', 'iff', 'dvf', 'hac-iff', 'hac-dvf'})} = 'open-loop'
+  args.type   char   {mustBeMember(args.type, {'open-loop', 'iff', 'dvf', 'hac-iff', 'hac-dvf', 'ref-track-L', 'ref-track-X'})} = 'open-loop'
 end
 
 controller = struct();
@@ -23,4 +23,8 @@ switch args.type
     controller.type = 3;
   case 'hac-dvf'
     controller.type = 4;
+  case 'ref-track-L'
+    controller.type = 5;
+  case 'ref-track-X'
+    controller.type = 6;
 end

src/initializeDisturbances.m

@@ -0,0 +1,20 @@
+function [disturbances] = initializeDisturbances(args)
+% initializeDisturbances - Initialize the disturbances
+%
+% Syntax: [disturbances] = initializeDisturbances(args)
+%
+% Inputs:
+%    - args -
+
+arguments
+  args.Fd     double  {mustBeNumeric, mustBeReal} = zeros(6,1)
+  args.Fd_t   double  {mustBeNumeric, mustBeReal} = 0
+  args.Dw     double  {mustBeNumeric, mustBeReal} = zeros(6,1)
+  args.Dw_t   double  {mustBeNumeric, mustBeReal} = 0
+end
+
+disturbances = struct();
+
+disturbances.Dw = timeseries([args.Dw], args.Dw_t);
+
+disturbances.Fd = timeseries([args.Fd], args.Fd_t);

src/initializeReferences.m

@@ -0,0 +1,33 @@
+function [references] = initializeReferences(stewart, args)
+% initializeReferences - Initialize the references
+%
+% Syntax: [references] = initializeReferences(args)
+%
+% Inputs:
+%    - args -
+
+arguments
+  stewart
+  args.t double {mustBeNumeric, mustBeReal} = 0
+  args.r double {mustBeNumeric, mustBeReal} = zeros(6, 1)
+end
+
+rL = zeros(6, length(args.t));
+
+for i = 1:length(args.t)
+  R = [cos(args.r(6,i)) -sin(args.r(6,i))  0;
+       sin(args.r(6,i))  cos(args.r(6,i))  0;
+       0           0           1] * ...
+      [cos(args.r(5,i))  0           sin(args.r(5,i));
+       0           1           0;
+      -sin(args.r(5,i))  0           cos(args.r(5,i))] * ...
+      [1           0           0;
+       0           cos(args.r(4,i)) -sin(args.r(4,i));
+       0           sin(args.r(4,i))  cos(args.r(4,i))];
+
+ [Li, dLi] = inverseKinematics(stewart, 'AP', [args.r(1,i); args.r(2,i); args.r(3,i)], 'ARB', R);
+ rL(:, i) = dLi;
+end
+
+references.r  = timeseries(args.r, args.t);
+references.rL = timeseries(rL, args.t);