classdef MotionIdentTwinCAT
   properties
       %% Parameters
       AmsNetId                 % AMS Net ID of target
       AdsPort                  % ADS port of target
       PathExtGen               % Path to external generator in PLC
       Units                    % Units       
       NameInputSignal          % name of input signal       
       IdInputSignal            % Id of input signal
       PeriodInputSignal        % period of input signal       
       CountRepeat              % count repeat input signal
       ControlMode              % Control Mode of loop
       AmplitudeInputSignal     % amplitude of input signal
       AmplAuxMove              % amplitude of auxiliary move
       FreqAuxMove              % Frequency of auxiliary movement in Hz (suppression of friction)
       DateTimeMd               % date and date of execiution of system indentification
       IsLinear                 % Is linear motor
       xScale                   % X-scale for position input/output system data
       vScale                   % V-scale for position input/output system data
       FeedConstant             % Feed constant
       TaskTime                 % cycle time of PLC
       NameAxis                 % name of axis
       StepPreTrigger           % Pre-trigger time for step response [s]
       StepTime                 % Duration of step response [s]
       StepInitLevel            % Initial level of step response [X]
       StepFinalLevel           % Final level of step response [X]
       Notes                    % notes
       LengthWindow             % Length of window for computing BodePlot
       ModeSpectAnalys          % Mode of spectral analysis for computing BodePlot
       CheckPhaseOverflow       % Check phase overflow
       OffsetIOdata             % Offset of input/output data. If offset is positive input data are shifted forward. If offset is negative input data are shifted backward.
       AllowedPositionRange     % Allowed position range
       EnablePositionRangeCheck % Enable position range check
       AllowedTorqueRangeIdent  % Allowed torque range during identification [°%]
       EnableTorqueRangeCheckIdent  % Enable torque range check during identification
       AllowedTorqueRangeStRes  % Allowed torque range during step response [°%]
       EnableTorqueRangeCheckStRes  % Enable torque range check during step response
       %% Data
       CommandValue              % all input values in matrix (at the time of sampling with cycle time)
       FeedbackValue             % all output values in matrix (at the time of sampling with cycle time)
       CommandValueInterest      % input values captured at the time of sampling input signal
       FeedbackValueInterest     % output values captured at the time of sampling input signal
       allCommandValue           % all input values in row vector (at the time of sampling with cycle time)
       allFeedbackValue          % all output values in row vector (at the time of sampling with cycle time)
       allCommandValueInterest   % input values captured at the time of sampling input signal in row vector
       allFeedbackValueInterest  % output values captured at the time of sampling input signal in row vector
       allCurrentValue           % all current values in row vector (at the time of sampling with cycle time)
       allCommandStepResValue    % all input values of step response
       allFeedbackStepResValue   % all output values of step response
       LengthInputSignal         % number bits of input signal
      
   end
   methods
      function obj = MotionIdentTwinCAT()
               obj.AmsNetId = '';                   % AMS Net ID of target
               obj.AdsPort = 0;                     % ADS port of target
               obj.PathExtGen = '';                 % Path to external generator in PLC
               obj.Units = 2;                       % Units       
               obj.NameInputSignal = '';            % name of input signal       
               obj.IdInputSignal = -1;              % Id of input signal
               obj.PeriodInputSignal = 0;           % period of input signal       
               obj.CountRepeat = 0;                 % count repeat input signal
               obj.ControlMode = 3;                 % Control Mode of loop
               obj.AmplitudeInputSignal = 0;        % amplitude of input signal
               obj.AmplAuxMove = 0;                 % amplitude of auxiliary move
               obj.FreqAuxMove = 0;                 % Frequency of auxiliary movement in Hz (suppression of friction)
               obj.DateTimeMd = '';                 % date and date of execiution of system indentification
               obj.IsLinear = false;                % Is linear motor
               obj.xScale = 0;                      % X-scale for position input/output system data
               obj.vScale = 0;                      % V-scale for position input/output system data
               obj.FeedConstant = 0;                % Feed constant
               obj.TaskTime = 0;                    % cycle time of PLC
               obj.NameAxis = '';                   % name of axis
               obj.StepPreTrigger = 0;              % Pre-trigger time for step response [s]
               obj.StepTime = 0;                    % Duration of step response [s]
               obj.StepInitLevel = 0;               % Initial level of step response [X]
               obj.StepFinalLevel = 0;              % Final level of step response [X]
               obj.Notes = '';                      % notes
               obj.LengthWindow = 0;                % Length of window for computing BodePlot
               obj.ModeSpectAnalys = 0;             % Mode of spectral analysis for computing BodePlot
               obj.CheckPhaseOverflow = false;      % Check phase overflow
               obj.OffsetIOdata = 0;                % Offset of input/output data. If offset is positive input data are shifted forward. If offset is negative input data are shifted backward.
               obj.AllowedPositionRange = 0;        % Allowed position range
               obj.EnablePositionRangeCheck = false;    % Enable position range check
               obj.AllowedTorqueRangeIdent = 0;     % Allowed torque range during identification [°%]
               obj.EnableTorqueRangeCheckIdent = false;  % Enable torque range check during identification
               obj.AllowedTorqueRangeStRes = 0;     % Allowed torque range during step response [°%]
               obj.EnableTorqueRangeCheckStRes = false; % Enable torque range check during step response    
               obj.LengthInputSignal = 0;           % number bits of input signal
      end
      
      function obj_ret = SetValues(obj, iAllCommandValue, iAllFeedbackValue) 
          
                [b,a] = butter(9,0.2);
                iAllCommandValue = filter(b,a,double(iAllCommandValue));
                iAllFeedbackValue = filter(b,a,double(iAllFeedbackValue));
            
            
              % row vectors with all values (at the time of sampling with cycle time)
              obj.allCommandValue = iAllCommandValue;           % all command values in vector
              obj.allFeedbackValue = iAllFeedbackValue;         % all feedback values in vector
              
               % matrix with all values (at the time of sampling with cycle time)            
              obj.CommandValue = zeros(obj.CountRepeat,obj.LengthInputSignal*(obj.PeriodInputSignal/obj.TaskTime));
              obj.FeedbackValue = zeros(obj.CountRepeat,obj.LengthInputSignal*(obj.PeriodInputSignal/obj.TaskTime));
              for i=1:obj.CountRepeat
                    obj.CommandValue(i,:) = obj.allCommandValue(((i-1)*obj.LengthInputSignal*(obj.PeriodInputSignal/obj.TaskTime)+1) : i*obj.LengthInputSignal*(obj.PeriodInputSignal/obj.TaskTime));   
                    obj.FeedbackValue(i,:) = obj.allFeedbackValue(((i-1)*obj.LengthInputSignal*(obj.PeriodInputSignal/obj.TaskTime)+1) : i*obj.LengthInputSignal*(obj.PeriodInputSignal/obj.TaskTime));
              end
              
              % matrix with intersting values (at the time of sampling input signal)
              obj.CommandValueInterest = zeros(size(obj.CommandValue,1),size(obj.CommandValue,2)/(obj.PeriodInputSignal/obj.TaskTime));
              obj.FeedbackValueInterest = zeros(size(obj.FeedbackValue,1),size(obj.FeedbackValue,2)/(obj.PeriodInputSignal/obj.TaskTime));
              for i = 1:size(obj.CommandValue,1)
                  for j = 1:(size(obj.CommandValue,2)/(obj.PeriodInputSignal/obj.TaskTime))
                      obj.CommandValueInterest(i,j) = obj.CommandValue(i,(j*(obj.PeriodInputSignal/obj.TaskTime)-((obj.PeriodInputSignal/obj.TaskTime)-1))); 
                      obj.FeedbackValueInterest(i,j) = obj.FeedbackValue(i,(j*(obj.PeriodInputSignal/obj.TaskTime)-((obj.PeriodInputSignal/obj.TaskTime)-1)));
                  end
              end       
                          
              % row vectors with intersting values (at the time of sampling input signal)
              obj.allCommandValueInterest = zeros(1,size(obj.CommandValueInterest,1)*size(obj.CommandValueInterest,2));         % interesting command values in vector
              obj.allFeedbackValueInterest = zeros(1,size(obj.FeedbackValueInterest,1)*size(obj.FeedbackValueInterest,2));      %  interesting feedback values in vector
              for i = 1:size(obj.CommandValueInterest,1)
                  for j = 1:size(obj.CommandValueInterest,2)
                      obj.allCommandValueInterest(((i-1)*size(obj.CommandValueInterest,2)) + j) = obj.CommandValueInterest(i,j);
                      obj.allFeedbackValueInterest(((i-1)*size(obj.FeedbackValueInterest,2)) + j) = obj.FeedbackValueInterest(i,j);
                  end
              end
              
              obj_ret = obj;
      end 
     
      function str = CreateTitle(obj, sLegend)
         % Creating title with properties of system identification
        if obj.ControlMode == 0
            UnitStr = '[°]';
        elseif obj.ControlMode == 1
            UnitStr = '[°/s]';
        elseif obj.ControlMode == 2
            UnitStr = '[Nm]';
        else
            UnitStr = '[-]';
        end
        str = sprintf('%s:\n Period of input signal:%3.1f [ms], Number of repeat: %dx, Amplitude of input signal: %4.2f%s, Length of input signal: %d',sLegend ,obj.PeriodInputSignal*1000, obj.CountRepeat, obj.AmplitudeInputSignal, UnitStr, obj.LengthInputSignal);
      end
    
   end
end
