├── .gitignore ├── 180HzPIDWith700HzNotchForTimeDelay.mat ├── GuideForSimCode.pdf ├── LICENSE ├── README.md ├── RunSim.m ├── RunThisFirst.m ├── backup ├── 180HzPIDWith700HzNotchForTimeDelay.mat ├── RunThisFirst.m ├── Untitled.m ├── Untitled3.m ├── createPlantModel.m ├── feedforwardAutoTuning.m ├── initiateSimulation.m ├── main.slx ├── plantModel.m ├── postprocessing │ ├── plotBode.m │ ├── plotError.m │ ├── plotVelError.m │ ├── postProcessing.m │ └── psdAnalysis.m └── traj │ ├── make2.m │ ├── make3.m │ ├── make4.m │ ├── make4_it.m │ ├── model_controller.slx │ ├── plotTraj.m │ ├── profile3.m │ ├── profile4.m │ ├── test.m │ ├── trajPowerSpectralAnalysis.m │ ├── trajTest.mdl.r2017a │ ├── trajTest.slx │ ├── trajTest.slx.r2016b │ └── trajTest.slx.r2017a ├── code.rar ├── createPlantModel.m ├── feedforwardAutoTuning.m ├── initiateSimulation.m ├── main.slx ├── plantModel.m ├── postprocessing ├── plotError.m └── postProcessing.m ├── preprocessingUfb.m ├── slprj └── sim │ └── varcache │ └── main │ ├── checksumOfCache.mat │ ├── tmwinternal │ └── simulink_cache.xml │ └── varInfo.mat └── traj ├── License ├── make2.m ├── make3.m ├── make4.m ├── make4_it.m ├── model_controller.slx ├── plotTraj.m ├── profile3.m ├── profile4.m ├── test.m ├── trajPowerSpectralAnalysis.m ├── trajTest.mdl.r2017a ├── trajTest.slx ├── trajTest.slx.r2016b └── trajTest.slx.r2017a /.gitignore: -------------------------------------------------------------------------------- 1 | *.asv 2 | *.slxc 3 | -------------------------------------------------------------------------------- /180HzPIDWith700HzNotchForTimeDelay.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DaiLuyaoME/Auto-tuning-of-model-based-feedforward-controller-in-ultraprecision-motion-systems/05efbdbd0e853b2d2ef3a15581306367d41388a3/180HzPIDWith700HzNotchForTimeDelay.mat -------------------------------------------------------------------------------- /GuideForSimCode.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DaiLuyaoME/Auto-tuning-of-model-based-feedforward-controller-in-ultraprecision-motion-systems/05efbdbd0e853b2d2ef3a15581306367d41388a3/GuideForSimCode.pdf -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2020 Dai Luyao 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Auto-tuning-of-model-based-feedforward-controller-in-ultraprecision-motion-systems 2 | * This repo is for the paper *Dai L, Li X, Zhu Y, Ming Zhang. Auto-tuning of model-based feedforward controller by feedback control signal in ultraprecision motion systems[J]. Mechanical Systems and Signal Processing, 2020, 142: 106764.* , https://www.sciencedirect.com/science/article/pii/S0888327020301503?via%3Dihub, DOI: https://doi.org/10.1016/j.ymssp.2020.106764 3 | * There is a step-by-step instruction to run the code, see GuideForSimCode.pdf. 4 | * The trajectory planning part is from https://ww2.mathworks.cn/matlabcentral/fileexchange/16352-advanced-setpoints-for-motion-systems?s_tid=prof_contriblnk, corresponding license is included in ./traj/License. 5 | -------------------------------------------------------------------------------- /RunSim.m: -------------------------------------------------------------------------------- 1 | %% 2 | accCoef = idealAccCoef; 3 | jerkCoef = idealJerkCoef; 4 | snapCoef = idealSnapCoef; 5 | 6 | trajParameters.dis = 0.04; 7 | trajParameters.vel = 0.25; 8 | trajParameters.acc = 10; 9 | trajParameters.jerk = 800; 10 | trajParameters.snap = 64000; 11 | 12 | sim('main',[0 0.24]); -------------------------------------------------------------------------------- /RunThisFirst.m: -------------------------------------------------------------------------------- 1 | addpath('traj','postprocessing'); -------------------------------------------------------------------------------- /backup/180HzPIDWith700HzNotchForTimeDelay.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DaiLuyaoME/Auto-tuning-of-model-based-feedforward-controller-in-ultraprecision-motion-systems/05efbdbd0e853b2d2ef3a15581306367d41388a3/backup/180HzPIDWith700HzNotchForTimeDelay.mat -------------------------------------------------------------------------------- /backup/RunThisFirst.m: -------------------------------------------------------------------------------- 1 | addpath('traj','postprocessing'); -------------------------------------------------------------------------------- /backup/Untitled.m: -------------------------------------------------------------------------------- 1 | feedbackControlSignal = ufb.signals.values; 2 | fbFilter = designfilt('lowpassiir', 'FilterOrder', 4, 'PassbandFrequency', 80, 'PassbandRipple', 0.01, 'SampleRate', 5000); 3 | ufbF = filtfilt(fbFilter,feedbackControlSignal); 4 | figure; 5 | plot([feedbackControlSignal,ufbF]); 6 | %% 7 | errorTuned = Err; 8 | %% 9 | plotError(Err.time,Err.signals.values*1e9,'tracking error under ideal feedforward'); 10 | xlim([acc.time(1),acc.time(end)]); 11 | hold on; 12 | plot(errorTuned.time,errorTuned.signals.values*1e9,'linewidth',2,'displayname','tracking error under tuned feedforward','linestyle','--'); 13 | legend1 = legend(gca,'show'); 14 | legend1.FontSize = 10; 15 | %% 16 | fn = 700; 17 | wn = fn * 2 * pi; 18 | zn = 0.03; 19 | m1 = 5; 20 | m2 = 20; 21 | 22 | k = wn^2 * m1 * m2 / (m1 + m2); 23 | c = 2 * zn * wn * m1 * m2 / (m1 + m2); 24 | %% 25 | ww = sqrt(k*(m1 + m2)/(m1*m2)); -------------------------------------------------------------------------------- /backup/Untitled3.m: -------------------------------------------------------------------------------- 1 | tempPD = pid(C); 2 | kp = tempPD.Kp; 3 | kd = tempPD.Kd; 4 | ki = tempPD.Ki; 5 | %% 6 | a1 = kd * kp / (ki * ki); 7 | a2 = 1/ki; 8 | a3 = kp^3 / (3*ki^3); 9 | %% 10 | tempMag = squeeze(freqresp(IOTransfer_dy2y,w)); 11 | max(abs(tempMag)); 12 | %% 13 | den = [ 1.000000000000, -1.620310200285, 0.713864305169, -0.093554104884 ]; 14 | num = [ 3.317148316240, -8.879887673303, 7.923413626761, -2.356564115411 ]; 15 | GcDis = tf(num,den,1/5000); -------------------------------------------------------------------------------- /backup/createPlantModel.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DaiLuyaoME/Auto-tuning-of-model-based-feedforward-controller-in-ultraprecision-motion-systems/05efbdbd0e853b2d2ef3a15581306367d41388a3/backup/createPlantModel.m -------------------------------------------------------------------------------- /backup/feedforwardAutoTuning.m: -------------------------------------------------------------------------------- 1 | %% tuning of acc ff 2 | feedbackControlSignal = ufb.signals.values; 3 | accSignal = acc.signals.values; 4 | simTime = ufb.time; 5 | 6 | startTime = 0.008; 7 | endTime = 0.042; 8 | 9 | beginIndex = find(simTime > startTime); 10 | endIndex = find(simTime > endTime); 11 | 12 | A = accSignal(beginIndex:endIndex); 13 | b = feedbackControlSignal(beginIndex:endIndex); 14 | accCoefEstimated = inv(A'*A) * A' * b; 15 | %% tuning of jerk ff 16 | feedbackControlSignal = ufb.signals.values; 17 | jerkSignal = jerk.signals.values; 18 | simTime = ufb.time; 19 | 20 | % startTime = 0.0; 21 | % endTime = 0.0472; 22 | 23 | beginIndex = find(simTime > startTime); 24 | endIndex = find(simTime > endTime); 25 | 26 | A = jerkSignal(beginIndex:endIndex); 27 | b = feedbackControlSignal(beginIndex:endIndex); 28 | jerkCoefEstimated = inv(A'*A) * A' * b; 29 | %% joint tuning of acc and jerk ff 30 | feedbackControlSignal = ufb.signals.values; 31 | accSignal = acc.signals.values; 32 | jerkSignal = jerk.signals.values; 33 | simTime = ufb.time; 34 | 35 | % startTime = 0.0; 36 | % endTime = 0.0472; 37 | 38 | beginIndex = find(simTime > startTime); 39 | endIndex = find(simTime > endTime); 40 | dataIndex = beginIndex:endIndex; 41 | 42 | A = [accSignal(dataIndex),jerkSignal(dataIndex)]; 43 | b = feedbackControlSignal(dataIndex); 44 | deltaEstimated = inv(A'*A) * A' * b; 45 | accCoefNew = accCoefEstimated + deltaEstimated(1); 46 | jerkCoefNew = jerkCoefEstimated + deltaEstimated(2); 47 | %% joint tuning of acc, jerk, snap ff 48 | accSignal = acc.signals.values; 49 | jerkSignal = jerk.signals.values; 50 | snapSignal = snap.signals.values; 51 | trajSignal = [accSignal,jerkSignal,snapSignal]; 52 | 53 | % startTime = 0.0; 54 | % endTime = 0.0472; 55 | beginIndex = find(simTime > startTime); 56 | endIndex = find(simTime > endTime); 57 | 58 | A = trajSignal(beginIndex:endIndex,:); 59 | b = feedbackControlSignal(beginIndex:endIndex); 60 | % b = ufbF; 61 | 62 | coefEstimated = inv(A'*A) * A' * b; 63 | 64 | accCoefLast = accCoefNew + coefEstimated(1); 65 | jerkCoefLast = jerkCoefNew + coefEstimated(2); 66 | snapCoefLast = coefEstimated(3); -------------------------------------------------------------------------------- /backup/initiateSimulation.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DaiLuyaoME/Auto-tuning-of-model-based-feedforward-controller-in-ultraprecision-motion-systems/05efbdbd0e853b2d2ef3a15581306367d41388a3/backup/initiateSimulation.m -------------------------------------------------------------------------------- /backup/main.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DaiLuyaoME/Auto-tuning-of-model-based-feedforward-controller-in-ultraprecision-motion-systems/05efbdbd0e853b2d2ef3a15581306367d41388a3/backup/main.slx -------------------------------------------------------------------------------- /backup/plantModel.m: -------------------------------------------------------------------------------- 1 | function [sys,x0,str,ts,simStateCompliance] = plantModel(t,x,u,flag,modelParameters) 2 | %SFUNTMPL General MATLAB S-Function Template 3 | % With MATLAB S-functions, you can define you own ordinary differential 4 | % equations (ODEs), discrete system equations, and/or just about 5 | % any type of algorithm to be used within a Simulink block diagram. 6 | % 7 | % The general form of an MATLAB S-function syntax is: 8 | % [SYS,X0,STR,TS,SIMSTATECOMPLIANCE] = SFUNC(T,X,U,FLAG,P1,...,Pn) 9 | % 10 | % What is returned by SFUNC at a given point in time, T, depends on the 11 | % value of the FLAG, the current state vector, X, and the current 12 | % input vector, U. 13 | % 14 | % FLAG RESULT DESCRIPTION 15 | % ----- ------ -------------------------------------------- 16 | % 0 [SIZES,X0,STR,TS] Initialization, return system sizes in SYS, 17 | % initial state in X0, state ordering strings 18 | % in STR, and sample times in TS. 19 | % 1 DX Return continuous state derivatives in SYS. 20 | % 2 DS Update discrete states SYS = X(n+1) 21 | % 3 Y Return outputs in SYS. 22 | % 4 TNEXT Return next time hit for variable step sample 23 | % time in SYS. 24 | % 5 Reserved for future (root finding). 25 | % 9 [] Termination, perform any cleanup SYS=[]. 26 | % 27 | % 28 | % The state vectors, X and X0 consists of continuous states followed 29 | % by discrete states. 30 | % 31 | % Optional parameters, P1,...,Pn can be provided to the S-function and 32 | % used during any FLAG operation. 33 | % 34 | % When SFUNC is called with FLAG = 0, the following information 35 | % should be returned: 36 | % 37 | % SYS(1) = Number of continuous states. 38 | % SYS(2) = Number of discrete states. 39 | % SYS(3) = Number of outputs. 40 | % SYS(4) = Number of inputs. 41 | % Any of the first four elements in SYS can be specified 42 | % as -1 indicating that they are dynamically sized. The 43 | % actual length for all other flags will be equal to the 44 | % length of the input, U. 45 | % SYS(5) = Reserved for root finding. Must be zero. 46 | % SYS(6) = Direct feedthrough flag (1=yes, 0=no). The s-function 47 | % has direct feedthrough if U is used during the FLAG=3 48 | % call. Setting this to 0 is akin to making a promise that 49 | % U will not be used during FLAG=3. If you break the promise 50 | % then unpredictable results will occur. 51 | % SYS(7) = Number of sample times. This is the number of rows in TS. 52 | % 53 | % 54 | % X0 = Initial state conditions or [] if no states. 55 | % 56 | % STR = State ordering strings which is generally specified as []. 57 | % 58 | % TS = An m-by-2 matrix containing the sample time 59 | % (period, offset) information. Where m = number of sample 60 | % times. The ordering of the sample times must be: 61 | % 62 | % TS = [0 0, : Continuous sample time. 63 | % 0 1, : Continuous, but fixed in minor step 64 | % sample time. 65 | % PERIOD OFFSET, : Discrete sample time where 66 | % PERIOD > 0 & OFFSET < PERIOD. 67 | % -2 0]; : Variable step discrete sample time 68 | % where FLAG=4 is used to get time of 69 | % next hit. 70 | % 71 | % There can be more than one sample time providing 72 | % they are ordered such that they are monotonically 73 | % increasing. Only the needed sample times should be 74 | % specified in TS. When specifying more than one 75 | % sample time, you must check for sample hits explicitly by 76 | % seeing if 77 | % abs(round((T-OFFSET)/PERIOD) - (T-OFFSET)/PERIOD) 78 | % is within a specified tolerance, generally 1e-8. This 79 | % tolerance is dependent upon your model's sampling times 80 | % and simulation time. 81 | % 82 | % You can also specify that the sample time of the S-function 83 | % is inherited from the driving block. For functions which 84 | % change during minor steps, this is done by 85 | % specifying SYS(7) = 1 and TS = [-1 0]. For functions which 86 | % are held during minor steps, this is done by specifying 87 | % SYS(7) = 1 and TS = [-1 1]. 88 | % 89 | % SIMSTATECOMPLIANCE = Specifices how to handle this block when saving and 90 | % restoring the complete simulation state of the 91 | % model. The allowed values are: 'DefaultSimState', 92 | % 'HasNoSimState' or 'DisallowSimState'. If this value 93 | % is not speficified, then the block's compliance with 94 | % simState feature is set to 'UknownSimState'. 95 | 96 | 97 | % Copyright 1990-2010 The MathWorks, Inc. 98 | 99 | % 100 | % The following outlines the general structure of an S-function. 101 | % 102 | switch flag 103 | 104 | %%%%%%%%%%%%%%%%%% 105 | % Initialization % 106 | %%%%%%%%%%%%%%%%%% 107 | case 0 108 | [sys,x0,str,ts,simStateCompliance]=mdlInitializeSizes; 109 | 110 | %%%%%%%%%%%%%%% 111 | % Derivatives % 112 | %%%%%%%%%%%%%%% 113 | case 1 114 | sys=mdlDerivatives(t,x,u,modelParameters); 115 | 116 | %%%%%%%%%% 117 | % Update % 118 | %%%%%%%%%% 119 | case 2 120 | sys=mdlUpdate(t,x,u); 121 | 122 | %%%%%%%%%%% 123 | % Outputs % 124 | %%%%%%%%%%% 125 | case 3 126 | sys=mdlOutputs(t,x,u); 127 | 128 | %%%%%%%%%%%%%%%%%%%%%%% 129 | % GetTimeOfNextVarHit % 130 | %%%%%%%%%%%%%%%%%%%%%%% 131 | case 4 132 | sys=mdlGetTimeOfNextVarHit(t,x,u); 133 | 134 | %%%%%%%%%%%%% 135 | % Terminate % 136 | %%%%%%%%%%%%% 137 | case 9 138 | sys=mdlTerminate(t,x,u); 139 | 140 | %%%%%%%%%%%%%%%%%%%% 141 | % Unexpected flags % 142 | %%%%%%%%%%%%%%%%%%%% 143 | otherwise 144 | DAStudio.error('Simulink:blocks:unhandledFlag', num2str(flag)); 145 | 146 | end 147 | 148 | % end sfuntmpl 149 | 150 | % 151 | %============================================================================= 152 | % mdlInitializeSizes 153 | % Return the sizes, initial conditions, and sample times for the S-function. 154 | %============================================================================= 155 | % 156 | function [sys,x0,str,ts,simStateCompliance]=mdlInitializeSizes 157 | 158 | % 159 | % call simsizes for a sizes structure, fill it in and convert it to a 160 | % sizes array. 161 | % 162 | % Note that in this example, the values are hard coded. This is not a 163 | % recommended practice as the characteristics of the block are typically 164 | % defined by the S-function parameters. 165 | % 166 | sizes = simsizes; 167 | 168 | sizes.NumContStates = 2; 169 | sizes.NumDiscStates = 0; 170 | sizes.NumOutputs = 2; 171 | sizes.NumInputs = 1; 172 | sizes.DirFeedthrough = 0; 173 | sizes.NumSampleTimes = 1; % at least one sample time is needed 174 | 175 | sys = simsizes(sizes); 176 | 177 | % 178 | % initialize the initial conditions 179 | % 180 | x0 = [0 0]; 181 | 182 | % 183 | % str is always an empty matrix 184 | % 185 | str = []; 186 | 187 | % 188 | % initialize the array of sample times 189 | % 190 | ts = [0 0]; 191 | 192 | % Specify the block simStateCompliance. The allowed values are: 193 | % 'UnknownSimState', < The default setting; warn and assume DefaultSimState 194 | % 'DefaultSimState', < Same sim state as a built-in block 195 | % 'HasNoSimState', < No sim state 196 | % 'DisallowSimState' < Error out when saving or restoring the model sim state 197 | simStateCompliance = 'UnknownSimState'; 198 | 199 | % end mdlInitializeSizes 200 | 201 | % 202 | %============================================================================= 203 | % mdlDerivatives 204 | % Return the derivatives for the continuous states. 205 | %============================================================================= 206 | % 207 | function sys=mdlDerivatives(t,x,u,modelParameters) 208 | 209 | sys(1) = x(2); 210 | sys(2) = u / modelParameters.mass; 211 | 212 | % end mdlDerivatives 213 | 214 | % 215 | %============================================================================= 216 | % mdlUpdate 217 | % Handle discrete state updates, sample time hits, and major time step 218 | % requirements. 219 | %============================================================================= 220 | % 221 | function sys=mdlUpdate(t,x,u) 222 | 223 | sys = []; 224 | 225 | % end mdlUpdate 226 | 227 | % 228 | %============================================================================= 229 | % mdlOutputs 230 | % Return the block outputs. 231 | %============================================================================= 232 | % 233 | function sys=mdlOutputs(t,x,u) 234 | 235 | sys = x; 236 | 237 | % end mdlOutputs 238 | 239 | % 240 | %============================================================================= 241 | % mdlGetTimeOfNextVarHit 242 | % Return the time of the next hit for this block. Note that the result is 243 | % absolute time. Note that this function is only used when you specify a 244 | % variable discrete-time sample time [-2 0] in the sample time array in 245 | % mdlInitializeSizes. 246 | %============================================================================= 247 | % 248 | function sys=mdlGetTimeOfNextVarHit(t,x,u) 249 | 250 | % sampleTime = 1; % Example, set the next hit to be one second later. 251 | % sys = t + sampleTime; 252 | sys = []; 253 | 254 | % end mdlGetTimeOfNextVarHit 255 | 256 | % 257 | %============================================================================= 258 | % mdlTerminate 259 | % Perform any end of simulation tasks. 260 | %============================================================================= 261 | % 262 | function sys=mdlTerminate(t,x,u) 263 | 264 | sys = []; 265 | 266 | % end mdlTerminate 267 | -------------------------------------------------------------------------------- /backup/postprocessing/plotBode.m: -------------------------------------------------------------------------------- 1 | function plotBode(G) 2 | h=bodeplot(G); 3 | setoptions(h,'FreqUnits','Hz'); 4 | h=gca; 5 | set(h,'LineWidth',2); 6 | end 7 | -------------------------------------------------------------------------------- /backup/postprocessing/plotError.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DaiLuyaoME/Auto-tuning-of-model-based-feedforward-controller-in-ultraprecision-motion-systems/05efbdbd0e853b2d2ef3a15581306367d41388a3/backup/postprocessing/plotError.m -------------------------------------------------------------------------------- /backup/postprocessing/plotVelError.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DaiLuyaoME/Auto-tuning-of-model-based-feedforward-controller-in-ultraprecision-motion-systems/05efbdbd0e853b2d2ef3a15581306367d41388a3/backup/postprocessing/plotVelError.m -------------------------------------------------------------------------------- /backup/postprocessing/postProcessing.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DaiLuyaoME/Auto-tuning-of-model-based-feedforward-controller-in-ultraprecision-motion-systems/05efbdbd0e853b2d2ef3a15581306367d41388a3/backup/postprocessing/postProcessing.m -------------------------------------------------------------------------------- /backup/postprocessing/psdAnalysis.m: -------------------------------------------------------------------------------- 1 | close all; 2 | %% 3 | errData = Err; 4 | time = Err.time; 5 | fs = 5000; 6 | startTime = 0.025; 7 | endTime = 0.04; 8 | dataIndex = time > startTime & time < endTime; 9 | data =errData.signals.values(dataIndex); 10 | dataTime = time(dataIndex); 11 | %% 12 | figure; 13 | plot(dataTime,data); 14 | %% 15 | figure; 16 | powerSpectralAnalysis(data,5000,1); 17 | figure; 18 | cumulativePowerSpectral(data,5000,1,1); 19 | 20 | 21 | -------------------------------------------------------------------------------- /backup/traj/make2.m: -------------------------------------------------------------------------------- 1 | function [t,ad,aa,tx,a,v,p,tt]=make2(varargin) 2 | 3 | % [t,ad,aa,tx,a,v,p,tt] = make2(p,v,a,Ts,plt) 4 | % 5 | % Calculate timing for symmetrical second order profiles .... 6 | % 7 | % inputs: 8 | % p = desired path (specify positive) [m] 9 | % v = velocity bound (specify positive) [m/s] 10 | % a = acceleration bound (specify positive) [m/s2] 11 | % Ts = sampling time (optional, if not specified or 0: continuous time) 12 | % plt = optional: if plt=0 no profile plot is generated, default: plt=1 13 | % 14 | % outputs: 15 | % t(1) = constant acceleration phase duration 16 | % t(2) = constant velocity phase duration (default 0) 17 | % 18 | % t1 19 | % a .-----. 20 | % | | 21 | % | | t2 22 | % -'-----'--------.-----.-- 23 | % | | 24 | % | | 25 | % -a '-----' 26 | % t1 27 | % 28 | % In case of discrete time, acceleration bound a is reduced to ad 29 | % 30 | % .... and calculate profiles: 31 | % 32 | % aa = acceleration profile suitable for simulink 33 | % 34 | % tx = time sequence for plotting profiles 35 | % a = acceleration profile 36 | % v = velocity profile 37 | % p = position profile 38 | % 39 | % tt = 4 switching times for profile: 40 | % 41 | % t0 t1 42 | % a .-----. 43 | % | | 44 | % | | t2 t3 45 | % -'-----'--------.-----.-- 46 | % | | 47 | % | | 48 | % -a '-----' 49 | % 50 | % 51 | % Note: coinciding switching times are not removed 52 | 53 | % 54 | % Copyright 2004, Paul Lambrechts, The MathWorks, Inc. 55 | % 56 | 57 | if nargin < 3 || nargin > 5 58 | help make2 59 | return 60 | else 61 | p=abs(varargin{1}); 62 | v=abs(varargin{2}); 63 | a=abs(varargin{3}); 64 | plt=1; 65 | if nargin < 4 66 | Ts=0; 67 | else 68 | Ts=abs(varargin{4}); 69 | if nargin == 5 70 | plt=varargin{5}; 71 | end 72 | end 73 | end 74 | 75 | ad = a; % required for discrete time calculations 76 | 77 | % Calculation t1 78 | t1 = (p/a)^(1/2) ; % largest t1 with bound on acceleration 79 | if Ts>0 80 | t1 = ceil(t1/Ts)*Ts; 81 | ad = p/(t1^2); 82 | end 83 | % velocity test 84 | if v < ad*t1 % v bound violated ? 85 | t1 = v/a ; % t1 with bound on velocity not violated 86 | if Ts>0 87 | t1 = ceil(t1/Ts)*Ts; 88 | ad = v/t1; 89 | end 90 | end 91 | a = ad; % as t1 is now fixed, ad is the new bound on acceleration 92 | 93 | % Calculation t2 94 | t2 = (p-a*t1^2)/v ; % largest t2 with bound on velocity 95 | if Ts>0 96 | t2 = ceil(t2/Ts)*Ts; 97 | ad = p/( t1^2 + t1*t2 ); 98 | end 99 | if abs(t2)<1e-12, t2=0; end % for continuous time case 100 | 101 | % All time intervals are now calculated 102 | t=[ t1 t2 ] ; 103 | 104 | % This error should never occur !! 105 | if min(t)<0 106 | disp('ERROR: negative values found') 107 | end 108 | 109 | % Finished. 110 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 111 | 112 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 113 | % Calculate profiles 114 | 115 | a = ad; 116 | 117 | tt= [0 1 1 2]*t(1) ... 118 | + [0 0 1 1]*t(2); 119 | 120 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 121 | % Generate Simulink look-up table 122 | at=[]; 123 | for i=1:4 124 | at = [at [1 1] * tt(i) ] ; 125 | end 126 | at = [at 1.5*tt(4)]; 127 | 128 | aa = [at ; [ 0 a a 0 0 -a -a 0 0 ] ]; 129 | 130 | if plt==0 % no plot required 131 | tx=[];a=[];v=[];p=[]; % dummy outputs 132 | return 133 | end 134 | 135 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 136 | % Generate profiles for plotting 137 | 138 | % Determine time grid for plotting 139 | if Ts==0 140 | Ts=t1/99.5; % continuous time accuracy for profile calculations 141 | while tt(4)/Ts > 5000 && Ts*2 < tt(2) 142 | Ts=Ts*2; % to prevent plot of more than 5000 points 143 | end 144 | else 145 | % Ts=Ts/10; % discrete time accuracy for profile calculations 146 | end 147 | 148 | % Determine continuous or discrete 149 | if max(abs( round(t/Ts)-t/Ts )) > 1e-12 % continuous 150 | 151 | disp('Calculating continuous time profiles') 152 | step = Ts; 153 | tx=(0:step:1.2*tt(4)); 154 | x=[]; 155 | for i=0:step:1.2*tt(4) 156 | j=find(i<=aa(1,:)); 157 | x=[x ; aa(2,j(1))]; 158 | end 159 | a=x; 160 | v=cumsum(a)*step; 161 | p=cumsum(v)*step; 162 | 163 | else % discrete 164 | 165 | disp('Calculating discrete time profiles') 166 | ttest=[tt 1.5*tt(4)]; 167 | len = round(1.2*tt(4)/Ts + 1); % length of profiles 168 | if len<100 % to make sure there are sufficient points to make a smooth plot 169 | Ts = Ts/round(100/len); 170 | len = round(1.2*tt(4)/Ts + 1); 171 | end 172 | xa = zeros(len,1); 173 | xv = xa; 174 | xp = xa; 175 | xa(1) = a; 176 | tx=(0:Ts:1.2*tt(4)+Ts/2); 177 | for time=Ts:Ts:(1.2*tt(4)+Ts/2) 178 | i = find( (time + Ts/2) <= ttest ); i = i(1)-1; 179 | k = round(time/Ts); 180 | if i==1 181 | xa(k+1) = a; 182 | elseif i==3 183 | xa(k+1) = -a; 184 | else 185 | xa(k+1) = 0; 186 | end 187 | xv(k+1) = xv(k) + xa(k)*Ts; 188 | xp(k+1) = xp(k) + xv(k)*Ts; 189 | end 190 | a=xa;v=xv;p=xp; 191 | 192 | end 193 | 194 | %close all 195 | %figure 196 | subplot(311);plot(tx,a,'k','LineWidth',1.5);hold on;plot([0 0],[-1 1]*max(a),'k--',[1 1]*max(tt),[-1 1]*max(a),'k--','LineWidth',1.5);grid on; axis([ [-0.01 1]*max(tx) [-1.1 1.1]*max(a)]); 197 | title('Second order trajectory profiles');ylabel('a [m/s2]'); 198 | subplot(312);plot(tx,v,'k','LineWidth',1.5);hold on;plot([0 0],[0 1]*max(v),'k--',[1 1]*max(tt),[0 1]*max(v),'k--','LineWidth',1.5);grid on; axis([ [-0.01 1]*max(tx) [-0.1 1.1]*max(v)]); 199 | ylabel('v [m/s]'); 200 | subplot(313);plot(tx,p,'k','LineWidth',1.5);hold on;plot([0 0],[0 1]*max(p),'k--',[1 1]*max(tt),[0 1]*max(p),'k--','LineWidth',1.5);grid on; axis([ [-0.01 1]*max(tx) [-0.1 1.1]*max(p)]); 201 | xlabel('time [s]');ylabel('x [m]'); 202 | %set(1,'position',[700 400 500 500]) 203 | set(1,'paperposition',[0 0 5 5]) 204 | 205 | return 206 | subplot(311); 207 | text(tt(1)+tt(4)/200,max(a)/5,'t_0'); 208 | text(tt(2)+tt(4)/200,max(a)/5,'t_1'); 209 | text(tt(3)+tt(4)/200,max(a)/5,'t_2'); 210 | text(tt(4)+tt(4)/200,max(a)/5,'t_3'); 211 | -------------------------------------------------------------------------------- /backup/traj/make3.m: -------------------------------------------------------------------------------- 1 | function [t,jd]=make3(varargin) 2 | 3 | % [t,jd] = make3(p,v,a,j,Ts) 4 | % 5 | % Calculate timing for symmetrical third order profiles. 6 | % 7 | % inputs: 8 | % p = desired path (specify positive) [m] 9 | % v = velocity bound (specify positive) [m/s] 10 | % a = acceleration bound (specify positive) [m/s2] 11 | % j = jerk bound (specify positive) [m/s3] 12 | % Ts = sampling time (optional, if not specified or 0: continuous time) 13 | % 14 | % outputs: 15 | % t(1) = constant jerk phase duration 16 | % t(2) = constant acceleration phase duration (default 0) 17 | % t(3) = constant velocity phase duration (default 0) 18 | % 19 | % t1 t1 20 | % j .-. .-. 21 | % | | | | 22 | % | | t2 t3 t2 | | 23 | % -'-'----.-.------.-.----' '-- 24 | % | | | | 25 | % | | | | 26 | % -j '-' '-' 27 | % t1 t1 28 | % 29 | % In case of discrete time, jerk bound j is reduced to jd 30 | 31 | % 32 | % Copyright 2004, Paul Lambrechts, The MathWorks, Inc. 33 | % 34 | 35 | if nargin < 4 || nargin > 5 36 | help make3 37 | return 38 | else 39 | p=abs(varargin{1}); 40 | v=abs(varargin{2}); 41 | a=abs(varargin{3}); 42 | j=abs(varargin{4}); 43 | if nargin == 4 44 | Ts=0; 45 | else 46 | Ts=abs(varargin{5}); 47 | end 48 | end 49 | 50 | if isempty(p) || isempty(v) || isempty(a) || isempty(j) || isempty(Ts) 51 | disp('ERROR: insufficient input for trajectory calculation') 52 | return 53 | end 54 | 55 | tol = eps; % tolerance required for continuous time calculations 56 | jd = j; % required for discrete time calculations 57 | 58 | % Calculation t1 59 | t1 = (p/(2*j))^(1/3) ; % largest t1 with bound on jerk 60 | if Ts>0 61 | t1 = ceil(t1/Ts)*Ts; 62 | jd = 1/2*p/(t1^3); 63 | end 64 | % velocity test 65 | if v < jd*t1^2 % v bound violated ? 66 | t1 = (v/j)^(1/2) ; % t1 with bound on velocity not violated 67 | if Ts>0 68 | t1 = ceil(t1/Ts)*Ts; 69 | jd = v/(t1^2); 70 | end 71 | end 72 | % acceleration test 73 | if a < jd*t1 % a bound violated ? 74 | t1 = a/j ; % t1 with bound on acceleration not violated 75 | if Ts>0 76 | t1 = ceil(t1/Ts)*Ts; 77 | jd = a/t1; 78 | end 79 | end 80 | j = jd; % as t1 is now fixed, jd is the new bound on jerk 81 | 82 | % Calculation t2 83 | t2 = (t1^2/4+p/j/t1)^(1/2) - 3/2*t1 ; % largest t2 with bound on acceleration 84 | if Ts>0 85 | t2 = ceil(t2/Ts)*Ts; 86 | jd = p/( 2*t1^3 + 3*t1^2*t2 + t1*t2^2 ); 87 | end 88 | if abs(t2)0 93 | t2 = ceil(t2/Ts)*Ts; 94 | jd = v/( t1^2 + t1*t2 ); 95 | end 96 | end 97 | if abs(t2)0 103 | t3 = ceil(t3/Ts)*Ts; 104 | jd = p/( 2*t1^3 + 3*t1^2*t2 + t1*t2^2 + t1^2*t3 + t1*t2*t3 ); 105 | end 106 | if abs(t3) 8 51 | help make4 52 | return 53 | else 54 | p=abs(varargin{1}); 55 | v=abs(varargin{2}); 56 | a=abs(varargin{3}); 57 | j=abs(varargin{4}); 58 | d=abs(varargin{5}); 59 | if nargin == 5 60 | Ts=0; r=eps; s=15; 61 | elseif nargin == 6 62 | Ts=abs(varargin{6}); 63 | r=eps; s=15; 64 | elseif nargin == 7 65 | Ts=abs(varargin{6}); 66 | r=abs(varargin{7}); 67 | s=15; 68 | elseif nargin == 8 69 | Ts=abs(varargin{6}); 70 | r=abs(varargin{7}); 71 | s=abs(varargin{8}); 72 | end 73 | end 74 | 75 | if isempty(p) || isempty(v) || isempty(a) || isempty(j) || isempty(d) || ... 76 | isempty(Ts) || isempty(r) || isempty(s) 77 | disp('ERROR: insufficient input for trajectory calculation') 78 | return 79 | end 80 | 81 | tol = eps; % tolerance required for continuous time calculations 82 | dd = d; % required for discrete time calculations 83 | 84 | % Calculation constant djerk phase duration: t1 85 | t1 = (1/8*p/d)^(1/4) ; % largest t1 with bound on derivative of jerk 86 | if Ts>0 87 | t1 = ceil(t1/Ts)*Ts; 88 | dd = 1/8*p/(t1^4); 89 | end 90 | % velocity test 91 | if v < 2*dd*t1^3 % v bound violated ? 92 | t1 = (1/2*v/d)^(1/3) ; % t1 with bound on velocity not violated 93 | if Ts>0 94 | t1 = ceil(t1/Ts)*Ts; 95 | dd = 1/2*v/(t1^3); 96 | end 97 | end 98 | % acceleration test 99 | if a < dd*t1^2 % a bound violated ? 100 | t1 = (a/d)^(1/2) ; % t1 with bound on acceleration not violated 101 | if Ts>0 102 | t1 = ceil(t1/Ts)*Ts; 103 | dd = a/(t1^2); 104 | end 105 | end 106 | % jerk test 107 | if j < dd*t1 % j bound violated ? 108 | t1 = j/d ; % t1 with bound on jerk not violated 109 | if Ts>0 110 | t1 = ceil(t1/Ts)*Ts; 111 | dd = j/t1; 112 | end 113 | end 114 | d = dd; % as t1 is now fixed, dd is the new bound on derivative of jerk 115 | 116 | % Calculation constant jerk phase duration: t2 117 | P = -1/9 * t1^2; % calculations to determine 118 | Q = -1/27 * t1^3 - p/(4*d*t1); % positive real solution of 119 | D = P^3 + Q^2; % third order polynomial... 120 | R = ( -Q + sqrt(D) )^(1/3); % 121 | t2 = R - P/R - 5/3*t1 ; % largest t2 with bound on jerk 122 | if Ts>0 123 | t2 = ceil(t2/Ts)*Ts; 124 | dd = p/( 8*t1^4 + 16*t1^3*t2 + 10*t1^2*t2^2 + 2*t1*t2^3 ); 125 | end 126 | if abs(t2)0 131 | t2 = ceil(t2/Ts)*Ts; 132 | dd = v/( 2*t1^3 + 3*t1^2*t2 + t1*t2^2 ); 133 | end 134 | end 135 | if abs(t2)0 140 | t2 = ceil(t2/Ts)*Ts; 141 | dd = a/( t1^2 + t1*t2 ); 142 | end 143 | end 144 | if abs(t2)0 153 | t3 = ceil(t3/Ts)*Ts; 154 | dd = p/( c1*t3^2 + c2*t3 + c3 ); 155 | end 156 | if abs(t3)0 161 | t3 = ceil(t3/Ts)*Ts; 162 | dd = v/( 2*t1^3 + 3*t1^2*t2 + t1*t2^2 + t1^2*t3 + t1*t2*t3 ); 163 | end 164 | end 165 | if abs(t3)0 171 | t4 = ceil(t4/Ts)*Ts; 172 | dd = p/( c1*t3^2 + c2*t3 + c3 + t4*(2*t1^3 + 3*t1^2*t2 + t1*t2^2 + t1^2*t3 + t1*t2*t3) ) ; 173 | end 174 | if abs(t4)0 186 | x=ceil(log10(dd)); % determine exponent of dd 187 | ddq=dd/10^x; % scale to 0-1 188 | ddq=round(ddq*10^s)/10^s; % round to s decimals 189 | ddq=ddq*10^x; 190 | % actual displacement obtained with quantized dd 191 | pp = ddq*( c1*t3^2 + c2*t3 + c3 + t4*(2*t1^3 + 3*t1^2*t2 + t1*t2^2 + t1^2*t3 + t1*t2*t3) ) ; 192 | dif=p-pp; % position error due to quantization of dd 193 | cnt=round(dif/r); % divided by resolution gives 'number of increments' 194 | % of required position correction 195 | % smooth correction obtained by dividing over entire trajectory duration 196 | tt = 8*t(1)+4*t(2)+2*t(3)+t(4); 197 | ti = tt/Ts; % should be integer number of samples 198 | cor1=sign(cnt)*floor(abs(cnt/ti))*ti; % we need cor1/ti increments correction at each 199 | % ... sample during trajectory 200 | cor2=cnt-cor1; % remaining correction: 1 increment per sample 201 | % ... during first part of trajectory 202 | dd=[ddq cor1 cor2 dd]; 203 | else 204 | dd=[dd 0 0 dd]; % continuous time result in same format 205 | end 206 | 207 | % Finished. 208 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 209 | -------------------------------------------------------------------------------- /backup/traj/make4_it.m: -------------------------------------------------------------------------------- 1 | function [t,dd]=make4_it(varargin) 2 | 3 | % [t,dd] = make4(p,v,a,j,d,Ts,r,s) 4 | % 5 | % Calculate timing for symmetrical 4th order profiles. 6 | % This is an obsolete routine using iterations to determine solution of 3d 7 | % order polynomial equation 8 | % 9 | % inputs: 10 | % p = desired path (specify positive) [m] 11 | % v = velocity bound (specify positive) [m/s] 12 | % a = acceleration bound (specify positive) [m/s2] 13 | % j = jerk bound (specify positive) [m/s3] 14 | % d = derivative of jerk bound (specify positive) [m/s4] 15 | % Ts = sampling time [s] (optional, if not specified or 0: continuous time) 16 | % r = position resolution [m] (optional, if not specified: 10*eps) 17 | % s = number of decimals for digitized 18 | % derivative of jerk bound (optional, if not specified: 15) 19 | % 20 | % outputs: 21 | % t(1) = constant djerk phase duration 22 | % t(2) = constant jerk phase duration 23 | % t(3) = constant acceleration phase duration 24 | % t(4) = constant velocity phase duration 25 | % 26 | % t1 t1 t1 t1 27 | % .-. .-. .-. .-. 28 | % | | | | | | | | 29 | % | |t2 t3 t2| | t4 t2| | t3 | |t2 30 | % '-'--.-.----.-.--' '---------.-.--'-'----'-'--.-.-- 31 | % | | | | | | | | 32 | % | | | | | | | | 33 | % '-' '-' '-' '-' 34 | % t1 t1 t1 t1 35 | % 36 | % In case of discrete time, derivative of jerk bound d is reduced to dd and 37 | % quantized to ddq using position resolution r and number of significant decimals s 38 | % Two position correction terms are calculated to 'repair' the position error 39 | % resulting from using ddq instead of dd: 40 | % cor1 gives the number of position increments that can equally be divided 41 | % over the entire trajectory duration 42 | % cor2 gives the remaining number of position increments 43 | % The result is given as: 44 | % dd = [ ddq cor1 cor2 dd ] 45 | % 46 | 47 | % 48 | % Copyright 2004, Paul Lambrechts, The MathWorks, Inc. 49 | % 50 | 51 | % Checking validity of inputs 52 | if nargin < 5 || nargin > 8 53 | help make4 54 | return 55 | else 56 | p=abs(varargin{1}); 57 | v=abs(varargin{2}); 58 | a=abs(varargin{3}); 59 | j=abs(varargin{4}); 60 | d=abs(varargin{5}); 61 | if nargin == 5 62 | Ts=0; r=eps; s=15; 63 | elseif nargin == 6 64 | Ts=abs(varargin{6}); 65 | r=eps; s=15; 66 | elseif nargin == 7 67 | Ts=abs(varargin{6}); 68 | r=abs(varargin{7}); 69 | s=15; 70 | elseif nargin == 8 71 | Ts=abs(varargin{6}); 72 | r=abs(varargin{7}); 73 | s=abs(varargin{8}); 74 | end 75 | end 76 | 77 | if isempty(p) || isempty(v) || isempty(a) || isempty(j) || isempty(d) || ... 78 | isempty(Ts) || isempty(r) || isempty(s) 79 | disp('ERROR: insufficient input for trajectory calculation') 80 | return 81 | end 82 | 83 | tol = eps; % tolerance required for continuous time calculations 84 | dd = d; % required for discrete time calculations 85 | 86 | % Calculation constant djerk phase duration: t1 87 | t1 = (1/8*p/d)^(1/4) ; % largest t1 with bound on derivative of jerk 88 | if Ts>0 89 | t1 = ceil(t1/Ts)*Ts; 90 | dd = 1/8*p/(t1^4); 91 | end 92 | % velocity test 93 | if v < 2*dd*t1^3 % v bound violated ? 94 | t1 = (1/2*v/d)^(1/3) ; % t1 with bound on velocity not violated 95 | if Ts>0 96 | t1 = ceil(t1/Ts)*Ts; 97 | dd = 1/2*v/(t1^3); 98 | end 99 | end 100 | % acceleration test 101 | if a < dd*t1^2 % a bound violated ? 102 | t1 = (a/d)^(1/2) ; % t1 with bound on acceleration not violated 103 | if Ts>0 104 | t1 = ceil(t1/Ts)*Ts; 105 | dd = a/(t1^2); 106 | end 107 | end 108 | % jerk test 109 | if j < dd*t1 % j bound violated ? 110 | t1 = j/d ; % t1 with bound on jerk not violated 111 | if Ts>0 112 | t1 = ceil(t1/Ts)*Ts; 113 | dd = j/t1; 114 | end 115 | end 116 | d = dd; % as t1 is now fixed, dd is the new bound on derivative of jerk 117 | 118 | % Calculation constant jerk phase duration: t2 119 | t2=solve3(p,d,t1,tol); % largest t2 with bound on jerk (see function below) 120 | if Ts>0 121 | t2 = ceil(t2/Ts)*Ts; 122 | dd = p/( 8*t1^4 + 16*t1^3*t2 + 10*t1^2*t2^2 + 2*t1*t2^3 ); 123 | end 124 | if abs(t2)0 129 | t2 = ceil(t2/Ts)*Ts; 130 | dd = v/( 2*t1^3 + 3*t1^2*t2 + t1*t2^2 ); 131 | end 132 | end 133 | if abs(t2)0 138 | t2 = ceil(t2/Ts)*Ts; 139 | dd = a/( t1^2 + t1*t2 ); 140 | end 141 | end 142 | if abs(t2)0 151 | t3 = ceil(t3/Ts)*Ts; 152 | dd = p/( c1*t3^2 + c2*t3 + c3 ); 153 | end 154 | if abs(t3)0 159 | t3 = ceil(t3/Ts)*Ts; 160 | dd = v/( 2*t1^3 + 3*t1^2*t2 + t1*t2^2 + t1^2*t3 + t1*t2*t3 ); 161 | end 162 | end 163 | if abs(t3)0 169 | t4 = ceil(t4/Ts)*Ts; 170 | dd = p/( c1*t3^2 + c2*t3 + c3 + t4*(2*t1^3 + 3*t1^2*t2 + t1*t2^2 + t1^2*t3 + t1*t2*t3) ) ; 171 | end 172 | if abs(t4)0 184 | x=ceil(log10(dd)); % determine exponent of dd 185 | ddq=dd/10^x; % scale to 0-1 186 | ddq=round(ddq*10^s)/10^s; % round to s decimals 187 | ddq=ddq*10^x; 188 | % actual displacement obtained with quantized dd 189 | pp = ddq*( c1*t3^2 + c2*t3 + c3 + t4*(2*t1^3 + 3*t1^2*t2 + t1*t2^2 + t1^2*t3 + t1*t2*t3) ) ; 190 | dif=p-pp; % position error due to quantization of dd 191 | cnt=round(dif/r); % divided by resolution gives 'number of increments' 192 | % of required position correction 193 | % smooth correction obtained by dividing over entire trajectory duration 194 | tt = 8*t(1)+4*t(2)+2*t(3)+t(4); 195 | ti = tt/Ts; % should be integer number of samples 196 | cor1=sign(cnt)*floor(abs(cnt/ti))*ti; % we need cor1/ti increments correction at each 197 | % ... sample during trajectory 198 | cor2=cnt-cor1; % remaining correction: 1 increment per sample 199 | % ... during first part of trajectory 200 | dd=[ddq cor1 cor2 dd]; 201 | else 202 | dd=[dd 0 0 dd]; % continuous time result in same format 203 | end 204 | 205 | % Finished. 206 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 207 | 208 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 209 | function t2=solve3(p,d,t1,tol) 210 | % 211 | % Solve third order equation for t2: 212 | % 213 | % t2^3 + (5*t1)*t2^2 + (8*t1^2)*t2 + (4*t1^3 - p/(2*d*t1)) 214 | % 215 | % inputs: 216 | % p = desired path 217 | % d = derivative of jerk bound 218 | % t1 = derivative of jerk time interval 219 | % tol = tolerance for t2 220 | % 221 | % outputs: 222 | % t2 = constant jerk time interval 223 | % 224 | 225 | a = 4*t1^3 - p/(2*d*t1) ; % must be <= 0 226 | b = 8*t1^2 ; % must be > 0 227 | c = 5*t1 ; % must be > 0 228 | if a>eps % if a>0 t1 is calculated wrongly (eps for numerical accuracy) 229 | disp('ERROR: wrong input') 230 | keyboard 231 | return 232 | end 233 | if abs(a)= 0 and result >= 0 242 | thig = min(thig); % upper bound 243 | yhig = thig^3 + c*thig^2 + b*thig + a; % . . . because yhig positive 244 | 245 | if thig > 0 246 | i=0; % counter to prevent infinite loop 247 | while ok ~= 1 248 | tnew = tlow + (tlow-thig)/(yhig-ylow)*ylow; 249 | % tnew = (tlow + thig)/2; 250 | ynew = tnew^3 + c*tnew^2 + b*tnew + a; 251 | if abs(ynew)>tol && i<100 252 | i=i+1; 253 | tlow=tnew;ylow=ynew; % ynew always <0 due to b,c positive 254 | % (monotonously increasing derivatives) 255 | else 256 | ok=1; 257 | if i==100 258 | disp('WARNING: accuracy not reached in maximal number of iterations'); 259 | else 260 | %disp(sprintf('Third order polynomial equation solved for positive real value')); 261 | %disp(sprintf('in %i iterations, with accuracy %g',i,abs(ynew))); 262 | end 263 | end 264 | end 265 | else 266 | tnew=0; 267 | end 268 | 269 | t2=tnew; 270 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 271 | -------------------------------------------------------------------------------- /backup/traj/model_controller.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DaiLuyaoME/Auto-tuning-of-model-based-feedforward-controller-in-ultraprecision-motion-systems/05efbdbd0e853b2d2ef3a15581306367d41388a3/backup/traj/model_controller.slx -------------------------------------------------------------------------------- /backup/traj/plotTraj.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DaiLuyaoME/Auto-tuning-of-model-based-feedforward-controller-in-ultraprecision-motion-systems/05efbdbd0e853b2d2ef3a15581306367d41388a3/backup/traj/plotTraj.m -------------------------------------------------------------------------------- /backup/traj/profile3.m: -------------------------------------------------------------------------------- 1 | function [jj,tx,j,a,v,p,tt]=profile3(t,j,acc,plt) 2 | 3 | % function [jj,tx,j,a,v,p,tt]=profile3(t,j,acc) 4 | % 5 | % Calculate symmetrical third order profiles from times: 6 | % 7 | % Inputs: 8 | % 9 | % t(1) = constant jerk phase duration 10 | % t(2) = constant acceleration phase duration (default 0) 11 | % t(3) = constant velocity phase duration (default 0) 12 | % 13 | % j = bound on jerk 14 | % acc = continuous time: accuracy for profiles: t(1)*acc = minimal timestep 15 | % discrete time: sample time 16 | % 17 | % Outputs: 18 | % 19 | % jj = derivative of jerk profile suitable for simulink 20 | % 21 | % tx = time sequence for plotting profiles 22 | % j = jerk profile 23 | % a = acceleration profile 24 | % v = velocity profile 25 | % p = position profile 26 | % 27 | % tt = 8 switching times for profile: 28 | % 29 | % 0 1 6 7 30 | % .-. .-. 31 | % | | | | 32 | % | | 2 3 4 5 | | 33 | % '-'--.-.----.-.--' '-- 34 | % | | | | 35 | % | | | | 36 | % '-' '-' 37 | % 38 | % Note: coinciding switching times are not removed 39 | 40 | % 41 | % Copyright 2004, Paul Lambrechts, The MathWorks, Inc. 42 | % 43 | 44 | if nargin < 3 || nargin > 4 45 | help profile3 46 | return 47 | end 48 | if nargin==3 49 | plt=1; 50 | end 51 | 52 | if length(t)==1 % min distance with max jerk 53 | tt= [0 1 1 2 2 3 3 4 ]*t; 54 | 55 | elseif length(t)==2 % constant acceleration phase 56 | tt= [0 1 1 2 2 3 3 4 ]*t(1) ... 57 | + [0 0 1 1 1 1 2 2 ]*t(2); 58 | 59 | elseif length(t)==3 % constant velocity phase 60 | tt= [0 1 1 2 2 3 3 4 ]*t(1) ... 61 | + [0 0 1 1 1 1 2 2 ]*t(2) ... 62 | + [0 0 0 0 1 1 1 1 ]*t(3) ; 63 | else 64 | return 65 | end 66 | 67 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 68 | % Generate Simulink look-up table 69 | jt=[]; 70 | for i=1:8 71 | jt = [jt [1 1] * tt(i) ] ; 72 | end 73 | jt = [jt 1.5*tt(8)]; 74 | 75 | jj = [jt ; [ 0 j j 0 0 -j -j 0 0 -j -j 0 0 j j 0 0 ] ]; 76 | 77 | if plt==0 % no plot required 78 | tx=[];j=[];a=[];v=[];p=[]; % dummy outputs 79 | return 80 | end 81 | 82 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 83 | % Generate profiles for plotting 84 | 85 | % Determine continuous or discrete 86 | if max(abs( round(t/acc)-t/acc )) > 1e-12 % continuous 87 | 88 | disp('Calculating continuous time profiles') 89 | step = t(1)*acc; 90 | tx=0:step:1.2*tt(8); 91 | x=[]; 92 | for i=0:step:1.2*tt(8) 93 | j=find(i<=jj(1,:)); 94 | x=[x ; jj(2,j(1))]; 95 | end 96 | j=x; 97 | a=cumsum(j)*step; 98 | v=cumsum(a)*step; 99 | p=cumsum(v)*step; 100 | 101 | else % discrete 102 | 103 | disp('Calculating discrete time profiles') 104 | Ts=acc; 105 | ttest=[tt 1.5*tt(8)]; 106 | len = round(1.2*tt(8)/Ts + 1); % length of profiles 107 | xj = zeros(len,1); 108 | xa = xj; 109 | xv = xj; 110 | xp = xj; 111 | xj(1) = j; 112 | tx=0:Ts:1.2*tt(8)+Ts/2; 113 | for time=Ts:Ts:(1.2*tt(8)+Ts/2) 114 | i = find( (time + Ts/2) <= ttest ); i = i(1)-1; 115 | k = round(time/Ts); 116 | if i==1 || i==7 117 | xj(k+1) = j; 118 | elseif i==3 || i==5 119 | xj(k+1) = -j; 120 | else 121 | xj(k+1) = 0; 122 | end 123 | xa(k+1) = xa(k) + xj(k)*Ts; 124 | xv(k+1) = xv(k) + xa(k)*Ts; 125 | xp(k+1) = xp(k) + xv(k)*Ts; 126 | end 127 | j=xj;a=xa;v=xv;p=xp; 128 | 129 | end 130 | 131 | %close all 132 | %figure 133 | subplot(411);plot(tx,j,'k','LineWidth',1.5);hold on;plot([0 0],[-1 1]*max(j),'k--',[1 1]*max(tt),[-1 1]*max(j),'k--','LineWidth',1.5);grid on; axis([ [-0.01 1]*max(tx) [-1.1 1.1]*max(j)]); 134 | title('Third order trajectory profiles');ylabel('j [m/s3]'); 135 | subplot(412);plot(tx,a,'k','LineWidth',1.5);hold on;plot([0 0],[-1 1]*max(a),'k--',[1 1]*max(tt),[-1 1]*max(a),'k--','LineWidth',1.5);grid on; axis([ [-0.01 1]*max(tx) [-1.1 1.1]*max(a)]); 136 | ylabel('a [m/s2]'); 137 | subplot(413);plot(tx,v,'k','LineWidth',1.5);hold on;plot([0 0],[0 1]*max(v),'k--',[1 1]*max(tt),[0 1]*max(v),'k--','LineWidth',1.5);grid on; axis([ [-0.01 1]*max(tx) [-0.1 1.1]*max(v)]); 138 | ylabel('v [m/s]'); 139 | subplot(414);plot(tx,p,'k','LineWidth',1.5);hold on;plot([0 0],[0 1]*max(p),'k--',[1 1]*max(tt),[0 1]*max(p),'k--','LineWidth',1.5);grid on; axis([ [-0.01 1]*max(tx) [-0.1 1.1]*max(p)]); 140 | xlabel('time [s]');ylabel('x [m]'); 141 | set(1,'position',[700 200 500 680]) 142 | set(1,'paperposition',[0 0 5 6.8]) 143 | 144 | subplot(411); 145 | text(tt(1)+tt(8)/200,max(j)/5,'t_0'); 146 | text(tt(2)+tt(8)/200,max(j)/5,'t_1'); 147 | text(tt(3) ,max(j)/5,'t_2'); 148 | text(tt(4) ,max(j)/5,'t_3'); 149 | text(tt(5) ,max(j)/5,'t_4'); 150 | text(tt(6) ,max(j)/5,'t_5'); 151 | text(tt(7)+tt(8)/200,max(j)/5,'t_6'); 152 | text(tt(8)+tt(8)/200,max(j)/5,'t_7'); 153 | -------------------------------------------------------------------------------- /backup/traj/profile4.m: -------------------------------------------------------------------------------- 1 | function [dj,tx,d,j,a,v,p,tt]=profile4(t,d,acc,plt) 2 | 3 | % function [dj,tx,d,j,a,v,p,tt]=profile4(t,d,acc) 4 | % 5 | % Calculate symmetrical fourth order profiles from times: 6 | % 7 | % Inputs: 8 | % 9 | % t(1) = constant djerk phase duration 10 | % t(2) = constant jerk phase duration (default 0) 11 | % t(3) = constant acceleration phase duration (default 0) 12 | % t(4) = constant velocity phase duration (default 0) 13 | % 14 | % d = bound on djerk 15 | % acc = continuous time: accuracy for profiles: t(1)*acc = minimal timestep 16 | % discrete time: sample time 17 | % 18 | % Outputs: 19 | % 20 | % dj = derivative of jerk profile suitable for simulink 21 | % 22 | % tx = time sequence for plotting profiles 23 | % d = derivative of jerk profile 24 | % j = jerk profile 25 | % a = acceleration profile 26 | % v = velocity profile 27 | % p = position profile 28 | % 29 | % tt = 16 switching times for profile: 30 | % 31 | % 0 1 6 7 10 11 12 13 32 | % .-. .-. .-. .-. 33 | % | | | | | | | | 34 | % | | 2 3 4 5 | | 8 9 | | | | 14 15 35 | % '-'--.-.----.-.--' '---------.-.--'-'----'-'--.-.-- 36 | % | | | | | | | | 37 | % | | | | | | | | 38 | % '-' '-' '-' '-' 39 | % 40 | % Note: coinciding switching times are not removed 41 | 42 | % 43 | % Copyright 2004, Paul Lambrechts, The MathWorks, Inc. 44 | % 45 | 46 | if nargin~=3 && nargin~=4 47 | help profile4 48 | return 49 | end 50 | if nargin==3 51 | plt=1; 52 | end 53 | 54 | 55 | if length(t)==1 % min distance with max djerk 56 | tt= [0 1 1 2 2 3 3 4 4 5 5 6 6 7 7 8]*t; 57 | % tt= [0 1 1 2 2 3 4 4 4 4 4 5 5 6 6 7]*t; 58 | 59 | elseif length(t)==2 % constant jerk phase 60 | tt= [0 1 1 2 2 3 3 4 4 5 5 6 6 7 7 8]*t(1) ... 61 | + [0 0 1 1 1 1 2 2 2 2 3 3 3 3 4 4]*t(2); 62 | 63 | elseif length(t)==3 % constant acceleration phase 64 | tt= [0 1 1 2 2 3 3 4 4 5 5 6 6 7 7 8]*t(1) ... 65 | + [0 0 1 1 1 1 2 2 2 2 3 3 3 3 4 4]*t(2) ... 66 | + [0 0 0 0 1 1 1 1 1 1 1 1 2 2 2 2]*t(3) ; 67 | 68 | elseif length(t)==4 % constant velocity phase 69 | tt= [0 1 1 2 2 3 3 4 4 5 5 6 6 7 7 8]*t(1) ... 70 | + [0 0 1 1 1 1 2 2 2 2 3 3 3 3 4 4]*t(2) ... 71 | + [0 0 0 0 1 1 1 1 1 1 1 1 2 2 2 2]*t(3) ... 72 | + [0 0 0 0 0 0 0 0 1 1 1 1 1 1 1 1]*t(4) ; 73 | 74 | else 75 | return 76 | end 77 | 78 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 79 | % Generate Simulink look-up table 80 | dt=[]; 81 | for i=1:16 82 | dt = [dt [1 1]*tt(i) ]; 83 | end 84 | dt = [dt 1.5*tt(16)]; 85 | 86 | dd = [0 d d 0]; 87 | dd = [ dd -dd -dd dd -dd dd dd -dd 0]; 88 | 89 | dj = [dt ; dd] ; 90 | 91 | if plt==0 % no plot required 92 | tx=[];d=[];j=[];a=[];v=[];p=[]; % dummy outputs 93 | return 94 | end 95 | 96 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 97 | % Generate profiles for plotting 98 | 99 | % Determine continuous or discrete 100 | if max(abs( round(t/acc)-t/acc )) > 1e-12 % continuous 101 | disp('Calculating continuous time profiles') 102 | step = t(1)*acc; 103 | tx=(0:step:1.2*tt(16))'; 104 | x=[]; 105 | for i=0:step:1.2*tt(16) 106 | j=find(i<=dj(1,:)); 107 | x=[x ; dj(2,j(1))]; 108 | end 109 | d=x; 110 | j=cumsum(d)*step; 111 | a=cumsum(j)*step; 112 | v=cumsum(a)*step; 113 | p=cumsum(v)*step; 114 | 115 | else % discrete 116 | 117 | disp('Calculating discrete time profiles') 118 | Ts=acc; 119 | ttest=[tt 1.5*tt(16)]; 120 | len = round(1.2*tt(16)/Ts + 1); % length of profiles 121 | xd = zeros(len,1); 122 | xj = xd; 123 | xa = xd; 124 | xv = xd; 125 | xp = xd; 126 | xd(1) = d; 127 | tx=(0:Ts:1.2*tt(16)+Ts/2)'; 128 | for time=Ts:Ts:1.2*tt(16)+Ts/2 129 | j = find( (time + Ts/2) <= ttest ); j = j(1)-1; 130 | k = round(time/Ts); 131 | if j==1 || j==7 || j==11 || j==13 132 | xd(k+1) = d; 133 | elseif j==3 || j==5 || j==9 || j==15 134 | xd(k+1) = -d; 135 | else 136 | xd(k+1) = 0; 137 | end 138 | xj(k+1) = xj(k) + xd(k)*Ts; 139 | xa(k+1) = xa(k) + xj(k)*Ts; 140 | xv(k+1) = xv(k) + xa(k)*Ts; 141 | xp(k+1) = xp(k) + xv(k)*Ts; 142 | end 143 | d=xd;j=xj;a=xa;v=xv;p=xp; 144 | 145 | dj(1,:)=dj(1,:)+Ts/2 ; % add Ts/2 to avoid numerical problems 146 | end 147 | dj=dj'; % to be compatible with Simulink 'From Workspace' block 148 | 149 | % figure 150 | %close all 151 | subplot(511);plot(tx,d,'k','LineWidth',1.5);hold on;plot([0 0],[-1 1]*max(d),'k--',[1 1]*max(tt),[-1 1]*max(d),'k--','LineWidth',1.5); grid on; axis([ [-0.01 1]*max(tx) [-1.1 1.1]*max(d)]); 152 | title('Fourth order trajectory profiles');ylabel('d [m/s4]'); 153 | subplot(512);plot(tx,j,'k','LineWidth',1.5);hold on;plot([0 0],[-1 1]*max(j),'k--',[1 1]*max(tt),[-1 1]*max(j),'k--','LineWidth',1.5);grid on; axis([ [-0.01 1]*max(tx) [-1.1 1.1]*max(j)]); 154 | ylabel('j [m/s3]'); 155 | subplot(513);plot(tx,a,'k','LineWidth',1.5);hold on;plot([0 0],[-1 1]*max(a),'k--',[1 1]*max(tt),[-1 1]*max(a),'k--','LineWidth',1.5);grid on; axis([ [-0.01 1]*max(tx) [-1.1 1.1]*max(a)]); 156 | ylabel('a [m/s2]'); 157 | subplot(514);plot(tx,v,'k','LineWidth',1.5);hold on;plot([0 0],[0 1]*max(v),'k--',[1 1]*max(tt),[0 1]*max(v),'k--','LineWidth',1.5);grid on; axis([ [-0.01 1]*max(tx) [-0.1 1.1]*max(v)]); 158 | ylabel('v [m/s]'); 159 | subplot(515);plot(tx,p,'k','LineWidth',1.5);hold on;plot([0 0],[0 1]*max(p),'k--',[1 1]*max(tt),[0 1]*max(p),'k--','LineWidth',1.5);grid on; axis([ [-0.01 1]*max(tx) [-0.1 1.1]*max(p)]); 160 | xlabel('time [s]');ylabel('x [m]'); 161 | set(1,'position',[700 100 500 860]) 162 | set(1,'paperposition',[0 0 5 8.6]) 163 | 164 | return 165 | subplot(511); 166 | text(tt( 1)+tt(16)/200,-max(d)/5,'t_0'); 167 | text(tt( 2)-tt(16)/200,-max(d)/5,'t_1'); 168 | text(tt( 3)-tt(16)/200, max(d)/5,'t_2'); 169 | text(tt( 4)-tt(16)/200, max(d)/5,'t_3'); 170 | text(tt( 5)-tt(16)/200, max(d)/5,'t_4'); 171 | text(tt( 6)-tt(16)/200, max(d)/5,'t_5'); 172 | text(tt( 7)-tt(16)/200,-max(d)/5,'t_6'); 173 | text(tt( 8)-tt(16)/200,-max(d)/5,'t_7'); 174 | text(tt( 9)-tt(16)/200, max(d)/5,'t_8'); 175 | text(tt(10)-tt(16)/200, max(d)/5,'t_9'); 176 | text(tt(11)-tt(16)/100,-max(d)/5,'t_1_0'); 177 | text(tt(12)-tt(16)/200,-max(d)/5,'t_1_1'); 178 | text(tt(13)-tt(16)/100,-max(d)/5,'t_1_2'); 179 | text(tt(14)-tt(16)/200,-max(d)/5,'t_1_3'); 180 | text(tt(15)-tt(16)/100, max(d)/5,'t_1_4'); 181 | text(tt(16)-tt(16)/200, max(d)/5,'t_1_5'); 182 | -------------------------------------------------------------------------------- /backup/traj/test.m: -------------------------------------------------------------------------------- 1 | plotTraj(acc.time,1000 * dis.signals.values,1000 * vel.signals.values,acc.signals.values,jerk.signals.values,snap.signals.values); 2 | %% 3 | plotTraj(acc.time,1000 * dis.signals.values,1000 * vel.signals.values,acc.signals.values,jerk.signals.values,snap.signals.values) -------------------------------------------------------------------------------- /backup/traj/trajPowerSpectralAnalysis.m: -------------------------------------------------------------------------------- 1 | figure; 2 | powerSpectralAnalysis(traj,5000); 3 | figure; 4 | cumulativePowerSpectral(traj,5000,1,1); 5 | %% 6 | global accStartTime; 7 | global accEndTime; 8 | -------------------------------------------------------------------------------- /backup/traj/trajTest.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DaiLuyaoME/Auto-tuning-of-model-based-feedforward-controller-in-ultraprecision-motion-systems/05efbdbd0e853b2d2ef3a15581306367d41388a3/backup/traj/trajTest.slx -------------------------------------------------------------------------------- /backup/traj/trajTest.slx.r2016b: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DaiLuyaoME/Auto-tuning-of-model-based-feedforward-controller-in-ultraprecision-motion-systems/05efbdbd0e853b2d2ef3a15581306367d41388a3/backup/traj/trajTest.slx.r2016b -------------------------------------------------------------------------------- /backup/traj/trajTest.slx.r2017a: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DaiLuyaoME/Auto-tuning-of-model-based-feedforward-controller-in-ultraprecision-motion-systems/05efbdbd0e853b2d2ef3a15581306367d41388a3/backup/traj/trajTest.slx.r2017a -------------------------------------------------------------------------------- /code.rar: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DaiLuyaoME/Auto-tuning-of-model-based-feedforward-controller-in-ultraprecision-motion-systems/05efbdbd0e853b2d2ef3a15581306367d41388a3/code.rar -------------------------------------------------------------------------------- /createPlantModel.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DaiLuyaoME/Auto-tuning-of-model-based-feedforward-controller-in-ultraprecision-motion-systems/05efbdbd0e853b2d2ef3a15581306367d41388a3/createPlantModel.m -------------------------------------------------------------------------------- /feedforwardAutoTuning.m: -------------------------------------------------------------------------------- 1 | %% code section A: tuning of acc ff 2 | feedbackControlSignal = ufb.signals.values; 3 | accSignal = acc.signals.values; 4 | simTime = ufb.time; 5 | 6 | startTime = 0.008; 7 | endTime = 0.042; 8 | 9 | beginIndex = find(simTime > startTime); 10 | endIndex = find(simTime > endTime); 11 | 12 | A = accSignal(beginIndex:endIndex); 13 | b = feedbackControlSignal(beginIndex:endIndex); 14 | accCoefEstimated = inv(A'*A) * A' * b; 15 | msgbox(sprintf('The tuned acceleration feedforward coefficient is %.4f kg',accCoefEstimated)); 16 | %% code section B: tuning of jerk ff 17 | feedbackControlSignal = ufb.signals.values; 18 | jerkSignal = jerk.signals.values; 19 | simTime = ufb.time; 20 | 21 | startTime = 0.0; 22 | endTime = 0.0472; 23 | 24 | beginIndex = find(simTime > startTime); 25 | endIndex = find(simTime > endTime); 26 | 27 | A = jerkSignal(beginIndex:endIndex); 28 | b = feedbackControlSignal(beginIndex:endIndex); 29 | jerkCoefEstimated = inv(A'*A) * A' * b; 30 | msgbox(sprintf('The tuned jerk feedforward coefficient is %.4f kg.s',jerkCoefEstimated)); 31 | %% code section C: joint tuning of acc and jerk ff 32 | feedbackControlSignal = ufb.signals.values; 33 | accSignal = acc.signals.values; 34 | jerkSignal = jerk.signals.values; 35 | simTime = ufb.time; 36 | 37 | startTime = 0.0; 38 | endTime = 0.0472; 39 | 40 | beginIndex = find(simTime > startTime); 41 | endIndex = find(simTime > endTime); 42 | dataIndex = beginIndex:endIndex; 43 | 44 | A = [accSignal(dataIndex),jerkSignal(dataIndex)]; 45 | b = feedbackControlSignal(dataIndex); 46 | deltaEstimated = inv(A'*A) * A' * b; 47 | accCoefNew = accCoefEstimated + deltaEstimated(1); 48 | jerkCoefNew = jerkCoefEstimated + deltaEstimated(2); 49 | msgbox(sprintf('The tuned acc feedforward coefficient is %.4f kg, jerk feedforward coefficient is %.4f kg.s',accCoefNew, jerkCoefNew)); 50 | %% code section D: joint tuning of acc, jerk, snap ff 51 | accSignal = acc.signals.values; 52 | jerkSignal = jerk.signals.values; 53 | snapSignal = snap.signals.values; 54 | trajSignal = [accSignal,jerkSignal,snapSignal]; 55 | 56 | startTime = 0.0; 57 | endTime = 0.0472; 58 | beginIndex = find(simTime > startTime); 59 | endIndex = find(simTime > endTime); 60 | 61 | A = trajSignal(beginIndex:endIndex,:); 62 | % b = feedbackControlSignal(beginIndex:endIndex); 63 | b = ufbF(beginIndex:endIndex,:); 64 | 65 | coefEstimated = inv(A'*A) * A' * b; 66 | 67 | accCoefLast = accCoefNew + coefEstimated(1); 68 | jerkCoefLast = jerkCoefNew + coefEstimated(2); 69 | snapCoefLast = coefEstimated(3); 70 | 71 | msgbox(sprintf('The tuned acc feedforward coefficient is %.4f kg, jerk feedforward coefficient is %.4f kg.s, snap feedforward coefficient is %.4e kg.s^2',accCoefLast, jerkCoefLast,snapCoefLast)); 72 | 73 | 74 | 75 | -------------------------------------------------------------------------------- /initiateSimulation.m: -------------------------------------------------------------------------------- 1 | % clear all; 2 | close all; 3 | clc; 4 | %% 5 | fs=5000; % sampling frequency 6 | Ts=1/fs; % sampling period 7 | 8 | %% 9 | fn = 700; % resonant frequency 10 | wn = fn * 2 * pi; 11 | zn = 0.03; % damping ratio 12 | m = [5,20]; 13 | Gp = createPlantModel(m,fn,zn,2); % non-colocated double-mass-block model 14 | 15 | s = tf('s'); 16 | tau = 1.5*Ts; 17 | delayModel = exp(-tau*s); % time delay model, the 0.5 sampling time delay is used to approxiamte zero order hold effect 18 | % delayModel = pade(delayModel,2); 19 | 20 | Gp = Gp * delayModel; % plant model with time delay considered 21 | % w = linspace(100 * 2 * pi,1500 * 2 * pi,2048); 22 | % figure;bodeplot(Gp,w); % plot Bode diagram of plant model 23 | 24 | load('180HzPIDWith700HzNotchForTimeDelay.mat'); % load feedback controller, the control bandwidth is 180Hz 25 | 26 | % ideal feedforward coefficients 27 | idealAccCoef = sum(m); 28 | idealJerkCoef = sum(m) * tau; 29 | idealSnapCoef = sum(m) * ( 1/wn.^2 + 0.5 * tau.^2); -------------------------------------------------------------------------------- /main.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DaiLuyaoME/Auto-tuning-of-model-based-feedforward-controller-in-ultraprecision-motion-systems/05efbdbd0e853b2d2ef3a15581306367d41388a3/main.slx -------------------------------------------------------------------------------- /plantModel.m: -------------------------------------------------------------------------------- 1 | function [sys,x0,str,ts,simStateCompliance] = plantModel(t,x,u,flag,modelParameters) 2 | %SFUNTMPL General MATLAB S-Function Template 3 | % With MATLAB S-functions, you can define you own ordinary differential 4 | % equations (ODEs), discrete system equations, and/or just about 5 | % any type of algorithm to be used within a Simulink block diagram. 6 | % 7 | % The general form of an MATLAB S-function syntax is: 8 | % [SYS,X0,STR,TS,SIMSTATECOMPLIANCE] = SFUNC(T,X,U,FLAG,P1,...,Pn) 9 | % 10 | % What is returned by SFUNC at a given point in time, T, depends on the 11 | % value of the FLAG, the current state vector, X, and the current 12 | % input vector, U. 13 | % 14 | % FLAG RESULT DESCRIPTION 15 | % ----- ------ -------------------------------------------- 16 | % 0 [SIZES,X0,STR,TS] Initialization, return system sizes in SYS, 17 | % initial state in X0, state ordering strings 18 | % in STR, and sample times in TS. 19 | % 1 DX Return continuous state derivatives in SYS. 20 | % 2 DS Update discrete states SYS = X(n+1) 21 | % 3 Y Return outputs in SYS. 22 | % 4 TNEXT Return next time hit for variable step sample 23 | % time in SYS. 24 | % 5 Reserved for future (root finding). 25 | % 9 [] Termination, perform any cleanup SYS=[]. 26 | % 27 | % 28 | % The state vectors, X and X0 consists of continuous states followed 29 | % by discrete states. 30 | % 31 | % Optional parameters, P1,...,Pn can be provided to the S-function and 32 | % used during any FLAG operation. 33 | % 34 | % When SFUNC is called with FLAG = 0, the following information 35 | % should be returned: 36 | % 37 | % SYS(1) = Number of continuous states. 38 | % SYS(2) = Number of discrete states. 39 | % SYS(3) = Number of outputs. 40 | % SYS(4) = Number of inputs. 41 | % Any of the first four elements in SYS can be specified 42 | % as -1 indicating that they are dynamically sized. The 43 | % actual length for all other flags will be equal to the 44 | % length of the input, U. 45 | % SYS(5) = Reserved for root finding. Must be zero. 46 | % SYS(6) = Direct feedthrough flag (1=yes, 0=no). The s-function 47 | % has direct feedthrough if U is used during the FLAG=3 48 | % call. Setting this to 0 is akin to making a promise that 49 | % U will not be used during FLAG=3. If you break the promise 50 | % then unpredictable results will occur. 51 | % SYS(7) = Number of sample times. This is the number of rows in TS. 52 | % 53 | % 54 | % X0 = Initial state conditions or [] if no states. 55 | % 56 | % STR = State ordering strings which is generally specified as []. 57 | % 58 | % TS = An m-by-2 matrix containing the sample time 59 | % (period, offset) information. Where m = number of sample 60 | % times. The ordering of the sample times must be: 61 | % 62 | % TS = [0 0, : Continuous sample time. 63 | % 0 1, : Continuous, but fixed in minor step 64 | % sample time. 65 | % PERIOD OFFSET, : Discrete sample time where 66 | % PERIOD > 0 & OFFSET < PERIOD. 67 | % -2 0]; : Variable step discrete sample time 68 | % where FLAG=4 is used to get time of 69 | % next hit. 70 | % 71 | % There can be more than one sample time providing 72 | % they are ordered such that they are monotonically 73 | % increasing. Only the needed sample times should be 74 | % specified in TS. When specifying more than one 75 | % sample time, you must check for sample hits explicitly by 76 | % seeing if 77 | % abs(round((T-OFFSET)/PERIOD) - (T-OFFSET)/PERIOD) 78 | % is within a specified tolerance, generally 1e-8. This 79 | % tolerance is dependent upon your model's sampling times 80 | % and simulation time. 81 | % 82 | % You can also specify that the sample time of the S-function 83 | % is inherited from the driving block. For functions which 84 | % change during minor steps, this is done by 85 | % specifying SYS(7) = 1 and TS = [-1 0]. For functions which 86 | % are held during minor steps, this is done by specifying 87 | % SYS(7) = 1 and TS = [-1 1]. 88 | % 89 | % SIMSTATECOMPLIANCE = Specifices how to handle this block when saving and 90 | % restoring the complete simulation state of the 91 | % model. The allowed values are: 'DefaultSimState', 92 | % 'HasNoSimState' or 'DisallowSimState'. If this value 93 | % is not speficified, then the block's compliance with 94 | % simState feature is set to 'UknownSimState'. 95 | 96 | 97 | % Copyright 1990-2010 The MathWorks, Inc. 98 | 99 | % 100 | % The following outlines the general structure of an S-function. 101 | % 102 | switch flag 103 | 104 | %%%%%%%%%%%%%%%%%% 105 | % Initialization % 106 | %%%%%%%%%%%%%%%%%% 107 | case 0 108 | [sys,x0,str,ts,simStateCompliance]=mdlInitializeSizes; 109 | 110 | %%%%%%%%%%%%%%% 111 | % Derivatives % 112 | %%%%%%%%%%%%%%% 113 | case 1 114 | sys=mdlDerivatives(t,x,u,modelParameters); 115 | 116 | %%%%%%%%%% 117 | % Update % 118 | %%%%%%%%%% 119 | case 2 120 | sys=mdlUpdate(t,x,u); 121 | 122 | %%%%%%%%%%% 123 | % Outputs % 124 | %%%%%%%%%%% 125 | case 3 126 | sys=mdlOutputs(t,x,u); 127 | 128 | %%%%%%%%%%%%%%%%%%%%%%% 129 | % GetTimeOfNextVarHit % 130 | %%%%%%%%%%%%%%%%%%%%%%% 131 | case 4 132 | sys=mdlGetTimeOfNextVarHit(t,x,u); 133 | 134 | %%%%%%%%%%%%% 135 | % Terminate % 136 | %%%%%%%%%%%%% 137 | case 9 138 | sys=mdlTerminate(t,x,u); 139 | 140 | %%%%%%%%%%%%%%%%%%%% 141 | % Unexpected flags % 142 | %%%%%%%%%%%%%%%%%%%% 143 | otherwise 144 | DAStudio.error('Simulink:blocks:unhandledFlag', num2str(flag)); 145 | 146 | end 147 | 148 | % end sfuntmpl 149 | 150 | % 151 | %============================================================================= 152 | % mdlInitializeSizes 153 | % Return the sizes, initial conditions, and sample times for the S-function. 154 | %============================================================================= 155 | % 156 | function [sys,x0,str,ts,simStateCompliance]=mdlInitializeSizes 157 | 158 | % 159 | % call simsizes for a sizes structure, fill it in and convert it to a 160 | % sizes array. 161 | % 162 | % Note that in this example, the values are hard coded. This is not a 163 | % recommended practice as the characteristics of the block are typically 164 | % defined by the S-function parameters. 165 | % 166 | sizes = simsizes; 167 | 168 | sizes.NumContStates = 2; 169 | sizes.NumDiscStates = 0; 170 | sizes.NumOutputs = 2; 171 | sizes.NumInputs = 1; 172 | sizes.DirFeedthrough = 0; 173 | sizes.NumSampleTimes = 1; % at least one sample time is needed 174 | 175 | sys = simsizes(sizes); 176 | 177 | % 178 | % initialize the initial conditions 179 | % 180 | x0 = [0 0]; 181 | 182 | % 183 | % str is always an empty matrix 184 | % 185 | str = []; 186 | 187 | % 188 | % initialize the array of sample times 189 | % 190 | ts = [0 0]; 191 | 192 | % Specify the block simStateCompliance. The allowed values are: 193 | % 'UnknownSimState', < The default setting; warn and assume DefaultSimState 194 | % 'DefaultSimState', < Same sim state as a built-in block 195 | % 'HasNoSimState', < No sim state 196 | % 'DisallowSimState' < Error out when saving or restoring the model sim state 197 | simStateCompliance = 'UnknownSimState'; 198 | 199 | % end mdlInitializeSizes 200 | 201 | % 202 | %============================================================================= 203 | % mdlDerivatives 204 | % Return the derivatives for the continuous states. 205 | %============================================================================= 206 | % 207 | function sys=mdlDerivatives(t,x,u,modelParameters) 208 | 209 | sys(1) = x(2); 210 | sys(2) = u / modelParameters.mass; 211 | 212 | % end mdlDerivatives 213 | 214 | % 215 | %============================================================================= 216 | % mdlUpdate 217 | % Handle discrete state updates, sample time hits, and major time step 218 | % requirements. 219 | %============================================================================= 220 | % 221 | function sys=mdlUpdate(t,x,u) 222 | 223 | sys = []; 224 | 225 | % end mdlUpdate 226 | 227 | % 228 | %============================================================================= 229 | % mdlOutputs 230 | % Return the block outputs. 231 | %============================================================================= 232 | % 233 | function sys=mdlOutputs(t,x,u) 234 | 235 | sys = x; 236 | 237 | % end mdlOutputs 238 | 239 | % 240 | %============================================================================= 241 | % mdlGetTimeOfNextVarHit 242 | % Return the time of the next hit for this block. Note that the result is 243 | % absolute time. Note that this function is only used when you specify a 244 | % variable discrete-time sample time [-2 0] in the sample time array in 245 | % mdlInitializeSizes. 246 | %============================================================================= 247 | % 248 | function sys=mdlGetTimeOfNextVarHit(t,x,u) 249 | 250 | % sampleTime = 1; % Example, set the next hit to be one second later. 251 | % sys = t + sampleTime; 252 | sys = []; 253 | 254 | % end mdlGetTimeOfNextVarHit 255 | 256 | % 257 | %============================================================================= 258 | % mdlTerminate 259 | % Perform any end of simulation tasks. 260 | %============================================================================= 261 | % 262 | function sys=mdlTerminate(t,x,u) 263 | 264 | sys = []; 265 | 266 | % end mdlTerminate 267 | -------------------------------------------------------------------------------- /postprocessing/plotError.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DaiLuyaoME/Auto-tuning-of-model-based-feedforward-controller-in-ultraprecision-motion-systems/05efbdbd0e853b2d2ef3a15581306367d41388a3/postprocessing/plotError.m -------------------------------------------------------------------------------- /postprocessing/postProcessing.m: -------------------------------------------------------------------------------- 1 | %% Code section A: plot tracking error 2 | plotError(Err.time,Err.signals.values*1e9,'tracking error'); 3 | xlim([acc.time(1),acc.time(end)]); 4 | hold on; 5 | temp1=max(abs(Err.signals.values*1e9)); 6 | temp2=max(abs(acc.signals.values)); 7 | temp3=max(abs(snap.signals.values)); 8 | temp4=max(abs(jerk.signals.values)); 9 | ratio=temp1/temp2; 10 | ratio31=temp1/temp3; 11 | ratio41 = temp1/temp4; 12 | 13 | % To comment these lines to disable scaled plot of corresponding 14 | % derivatives with tracking error. 15 | %plot(acc.time,ratio*acc.signals.values,'DisplayName','scaled acceleration','LineWidth',2); 16 | plot(snap.time,ratio31*snap.signals.values,'DisplayName','scaled snap','LineWidth',2); 17 | % plot(jerk.time,ratio41*jerk.signals.values,'DisplayName','scaled jerk','LineWidth',2); 18 | 19 | 20 | legend1 = legend(gca,'show'); 21 | legend1.FontSize = 22; 22 | 23 | %% Code section B: plot feedback control signal 24 | figure; 25 | plot(ufb.time,ufb.signals.values,'displayname','feedback control signal','linewidth',2); 26 | xlim([ufb.time(1),ufb.time(end)]); 27 | hold on; 28 | temp1=max(abs(ufb.signals.values)); 29 | temp2=max(abs(acc.signals.values)); 30 | temp3=max(abs(snap.signals.values)); 31 | temp4=max(abs(jerk.signals.values)); 32 | ratio=temp1/temp2; 33 | ratio31=temp1/temp3; 34 | ratio41 = temp1/temp4; 35 | 36 | % To comment these lines to disable scaled plot of corresponding 37 | % derivatives with feedback control signal. 38 | % plot(acc.time,ratio*acc.signals.values,'DisplayName','scaled acceleration','LineWidth',2); 39 | % plot(acc.time,ratio*acc.signals.values,'DisplayName','scaled acceleration','LineWidth',2,'linestyle','--'); 40 | % plot(snap.time,ratio31*snap.signals.values,'DisplayName','scaled snap','LineWidth',2); 41 | % plot(jerk.time, ratio41*jerk.signals.values,'DisplayName','scaled jerk','LineWidth',2); 42 | 43 | legend1 = legend(gca,'show'); 44 | xlabel('time (s)','fontsize',20); 45 | h = ylabel('control signal (N)','fontsize',20); 46 | set(gca,'fontsize',16); 47 | legend1.FontSize = 22; 48 | 49 | -------------------------------------------------------------------------------- /preprocessingUfb.m: -------------------------------------------------------------------------------- 1 | feedbackControlSignal = ufb.signals.values; 2 | fbFilter = designfilt('lowpassiir', 'FilterOrder', 4, 'PassbandFrequency', 80, 'PassbandRipple', 0.01, 'SampleRate', 5000); 3 | ufbF = filtfilt(fbFilter,feedbackControlSignal); 4 | figure; 5 | plot(snap.time,[feedbackControlSignal,ufbF],'linewidth',2); 6 | hold on; 7 | ratio31 = max(abs(feedbackControlSignal)) / max(snap.signals.values); 8 | plot(snap.time,ratio31*snap.signals.values,'DisplayName','scaled snap','LineWidth',2); 9 | %% 10 | errorTuned = Err; 11 | %% 12 | plotError(Err.time,Err.signals.values*1e9,'tracking error under ideal feedforward'); 13 | xlim([acc.time(1),acc.time(end)]); 14 | hold on; 15 | plot(errorTuned.time,errorTuned.signals.values*1e9,'linewidth',2,'displayname','tracking error under tuned feedforward','linestyle','--'); 16 | legend1 = legend(gca,'show'); 17 | legend1.FontSize = 10; 18 | -------------------------------------------------------------------------------- /slprj/sim/varcache/main/checksumOfCache.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DaiLuyaoME/Auto-tuning-of-model-based-feedforward-controller-in-ultraprecision-motion-systems/05efbdbd0e853b2d2ef3a15581306367d41388a3/slprj/sim/varcache/main/checksumOfCache.mat -------------------------------------------------------------------------------- /slprj/sim/varcache/main/tmwinternal/simulink_cache.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | XnarYBFknt26TmpOneVqJg== 5 | 6 | -------------------------------------------------------------------------------- /slprj/sim/varcache/main/varInfo.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DaiLuyaoME/Auto-tuning-of-model-based-feedforward-controller-in-ultraprecision-motion-systems/05efbdbd0e853b2d2ef3a15581306367d41388a3/slprj/sim/varcache/main/varInfo.mat -------------------------------------------------------------------------------- /traj/License: -------------------------------------------------------------------------------- 1 | Copyright (c) 2016, The MathWorks, Inc. 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | * In all cases, the software is, and all modifications and derivatives of the 14 | software shall be, licensed to you solely for use in conjunction with 15 | MathWorks products and service offerings. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE 21 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 22 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 23 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 24 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 25 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 26 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | -------------------------------------------------------------------------------- /traj/make2.m: -------------------------------------------------------------------------------- 1 | function [t,ad,aa,tx,a,v,p,tt]=make2(varargin) 2 | 3 | % [t,ad,aa,tx,a,v,p,tt] = make2(p,v,a,Ts,plt) 4 | % 5 | % Calculate timing for symmetrical second order profiles .... 6 | % 7 | % inputs: 8 | % p = desired path (specify positive) [m] 9 | % v = velocity bound (specify positive) [m/s] 10 | % a = acceleration bound (specify positive) [m/s2] 11 | % Ts = sampling time (optional, if not specified or 0: continuous time) 12 | % plt = optional: if plt=0 no profile plot is generated, default: plt=1 13 | % 14 | % outputs: 15 | % t(1) = constant acceleration phase duration 16 | % t(2) = constant velocity phase duration (default 0) 17 | % 18 | % t1 19 | % a .-----. 20 | % | | 21 | % | | t2 22 | % -'-----'--------.-----.-- 23 | % | | 24 | % | | 25 | % -a '-----' 26 | % t1 27 | % 28 | % In case of discrete time, acceleration bound a is reduced to ad 29 | % 30 | % .... and calculate profiles: 31 | % 32 | % aa = acceleration profile suitable for simulink 33 | % 34 | % tx = time sequence for plotting profiles 35 | % a = acceleration profile 36 | % v = velocity profile 37 | % p = position profile 38 | % 39 | % tt = 4 switching times for profile: 40 | % 41 | % t0 t1 42 | % a .-----. 43 | % | | 44 | % | | t2 t3 45 | % -'-----'--------.-----.-- 46 | % | | 47 | % | | 48 | % -a '-----' 49 | % 50 | % 51 | % Note: coinciding switching times are not removed 52 | 53 | % 54 | % Copyright 2004, Paul Lambrechts, The MathWorks, Inc. 55 | % 56 | 57 | if nargin < 3 || nargin > 5 58 | help make2 59 | return 60 | else 61 | p=abs(varargin{1}); 62 | v=abs(varargin{2}); 63 | a=abs(varargin{3}); 64 | plt=1; 65 | if nargin < 4 66 | Ts=0; 67 | else 68 | Ts=abs(varargin{4}); 69 | if nargin == 5 70 | plt=varargin{5}; 71 | end 72 | end 73 | end 74 | 75 | ad = a; % required for discrete time calculations 76 | 77 | % Calculation t1 78 | t1 = (p/a)^(1/2) ; % largest t1 with bound on acceleration 79 | if Ts>0 80 | t1 = ceil(t1/Ts)*Ts; 81 | ad = p/(t1^2); 82 | end 83 | % velocity test 84 | if v < ad*t1 % v bound violated ? 85 | t1 = v/a ; % t1 with bound on velocity not violated 86 | if Ts>0 87 | t1 = ceil(t1/Ts)*Ts; 88 | ad = v/t1; 89 | end 90 | end 91 | a = ad; % as t1 is now fixed, ad is the new bound on acceleration 92 | 93 | % Calculation t2 94 | t2 = (p-a*t1^2)/v ; % largest t2 with bound on velocity 95 | if Ts>0 96 | t2 = ceil(t2/Ts)*Ts; 97 | ad = p/( t1^2 + t1*t2 ); 98 | end 99 | if abs(t2)<1e-12, t2=0; end % for continuous time case 100 | 101 | % All time intervals are now calculated 102 | t=[ t1 t2 ] ; 103 | 104 | % This error should never occur !! 105 | if min(t)<0 106 | disp('ERROR: negative values found') 107 | end 108 | 109 | % Finished. 110 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 111 | 112 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 113 | % Calculate profiles 114 | 115 | a = ad; 116 | 117 | tt= [0 1 1 2]*t(1) ... 118 | + [0 0 1 1]*t(2); 119 | 120 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 121 | % Generate Simulink look-up table 122 | at=[]; 123 | for i=1:4 124 | at = [at [1 1] * tt(i) ] ; 125 | end 126 | at = [at 1.5*tt(4)]; 127 | 128 | aa = [at ; [ 0 a a 0 0 -a -a 0 0 ] ]; 129 | 130 | if plt==0 % no plot required 131 | tx=[];a=[];v=[];p=[]; % dummy outputs 132 | return 133 | end 134 | 135 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 136 | % Generate profiles for plotting 137 | 138 | % Determine time grid for plotting 139 | if Ts==0 140 | Ts=t1/99.5; % continuous time accuracy for profile calculations 141 | while tt(4)/Ts > 5000 && Ts*2 < tt(2) 142 | Ts=Ts*2; % to prevent plot of more than 5000 points 143 | end 144 | else 145 | % Ts=Ts/10; % discrete time accuracy for profile calculations 146 | end 147 | 148 | % Determine continuous or discrete 149 | if max(abs( round(t/Ts)-t/Ts )) > 1e-12 % continuous 150 | 151 | disp('Calculating continuous time profiles') 152 | step = Ts; 153 | tx=(0:step:1.2*tt(4)); 154 | x=[]; 155 | for i=0:step:1.2*tt(4) 156 | j=find(i<=aa(1,:)); 157 | x=[x ; aa(2,j(1))]; 158 | end 159 | a=x; 160 | v=cumsum(a)*step; 161 | p=cumsum(v)*step; 162 | 163 | else % discrete 164 | 165 | disp('Calculating discrete time profiles') 166 | ttest=[tt 1.5*tt(4)]; 167 | len = round(1.2*tt(4)/Ts + 1); % length of profiles 168 | if len<100 % to make sure there are sufficient points to make a smooth plot 169 | Ts = Ts/round(100/len); 170 | len = round(1.2*tt(4)/Ts + 1); 171 | end 172 | xa = zeros(len,1); 173 | xv = xa; 174 | xp = xa; 175 | xa(1) = a; 176 | tx=(0:Ts:1.2*tt(4)+Ts/2); 177 | for time=Ts:Ts:(1.2*tt(4)+Ts/2) 178 | i = find( (time + Ts/2) <= ttest ); i = i(1)-1; 179 | k = round(time/Ts); 180 | if i==1 181 | xa(k+1) = a; 182 | elseif i==3 183 | xa(k+1) = -a; 184 | else 185 | xa(k+1) = 0; 186 | end 187 | xv(k+1) = xv(k) + xa(k)*Ts; 188 | xp(k+1) = xp(k) + xv(k)*Ts; 189 | end 190 | a=xa;v=xv;p=xp; 191 | 192 | end 193 | 194 | %close all 195 | %figure 196 | subplot(311);plot(tx,a,'k','LineWidth',1.5);hold on;plot([0 0],[-1 1]*max(a),'k--',[1 1]*max(tt),[-1 1]*max(a),'k--','LineWidth',1.5);grid on; axis([ [-0.01 1]*max(tx) [-1.1 1.1]*max(a)]); 197 | title('Second order trajectory profiles');ylabel('a [m/s2]'); 198 | subplot(312);plot(tx,v,'k','LineWidth',1.5);hold on;plot([0 0],[0 1]*max(v),'k--',[1 1]*max(tt),[0 1]*max(v),'k--','LineWidth',1.5);grid on; axis([ [-0.01 1]*max(tx) [-0.1 1.1]*max(v)]); 199 | ylabel('v [m/s]'); 200 | subplot(313);plot(tx,p,'k','LineWidth',1.5);hold on;plot([0 0],[0 1]*max(p),'k--',[1 1]*max(tt),[0 1]*max(p),'k--','LineWidth',1.5);grid on; axis([ [-0.01 1]*max(tx) [-0.1 1.1]*max(p)]); 201 | xlabel('time [s]');ylabel('x [m]'); 202 | %set(1,'position',[700 400 500 500]) 203 | set(1,'paperposition',[0 0 5 5]) 204 | 205 | return 206 | subplot(311); 207 | text(tt(1)+tt(4)/200,max(a)/5,'t_0'); 208 | text(tt(2)+tt(4)/200,max(a)/5,'t_1'); 209 | text(tt(3)+tt(4)/200,max(a)/5,'t_2'); 210 | text(tt(4)+tt(4)/200,max(a)/5,'t_3'); 211 | -------------------------------------------------------------------------------- /traj/make3.m: -------------------------------------------------------------------------------- 1 | function [t,jd]=make3(varargin) 2 | 3 | % [t,jd] = make3(p,v,a,j,Ts) 4 | % 5 | % Calculate timing for symmetrical third order profiles. 6 | % 7 | % inputs: 8 | % p = desired path (specify positive) [m] 9 | % v = velocity bound (specify positive) [m/s] 10 | % a = acceleration bound (specify positive) [m/s2] 11 | % j = jerk bound (specify positive) [m/s3] 12 | % Ts = sampling time (optional, if not specified or 0: continuous time) 13 | % 14 | % outputs: 15 | % t(1) = constant jerk phase duration 16 | % t(2) = constant acceleration phase duration (default 0) 17 | % t(3) = constant velocity phase duration (default 0) 18 | % 19 | % t1 t1 20 | % j .-. .-. 21 | % | | | | 22 | % | | t2 t3 t2 | | 23 | % -'-'----.-.------.-.----' '-- 24 | % | | | | 25 | % | | | | 26 | % -j '-' '-' 27 | % t1 t1 28 | % 29 | % In case of discrete time, jerk bound j is reduced to jd 30 | 31 | % 32 | % Copyright 2004, Paul Lambrechts, The MathWorks, Inc. 33 | % 34 | 35 | if nargin < 4 || nargin > 5 36 | help make3 37 | return 38 | else 39 | p=abs(varargin{1}); 40 | v=abs(varargin{2}); 41 | a=abs(varargin{3}); 42 | j=abs(varargin{4}); 43 | if nargin == 4 44 | Ts=0; 45 | else 46 | Ts=abs(varargin{5}); 47 | end 48 | end 49 | 50 | if isempty(p) || isempty(v) || isempty(a) || isempty(j) || isempty(Ts) 51 | disp('ERROR: insufficient input for trajectory calculation') 52 | return 53 | end 54 | 55 | tol = eps; % tolerance required for continuous time calculations 56 | jd = j; % required for discrete time calculations 57 | 58 | % Calculation t1 59 | t1 = (p/(2*j))^(1/3) ; % largest t1 with bound on jerk 60 | if Ts>0 61 | t1 = ceil(t1/Ts)*Ts; 62 | jd = 1/2*p/(t1^3); 63 | end 64 | % velocity test 65 | if v < jd*t1^2 % v bound violated ? 66 | t1 = (v/j)^(1/2) ; % t1 with bound on velocity not violated 67 | if Ts>0 68 | t1 = ceil(t1/Ts)*Ts; 69 | jd = v/(t1^2); 70 | end 71 | end 72 | % acceleration test 73 | if a < jd*t1 % a bound violated ? 74 | t1 = a/j ; % t1 with bound on acceleration not violated 75 | if Ts>0 76 | t1 = ceil(t1/Ts)*Ts; 77 | jd = a/t1; 78 | end 79 | end 80 | j = jd; % as t1 is now fixed, jd is the new bound on jerk 81 | 82 | % Calculation t2 83 | t2 = (t1^2/4+p/j/t1)^(1/2) - 3/2*t1 ; % largest t2 with bound on acceleration 84 | if Ts>0 85 | t2 = ceil(t2/Ts)*Ts; 86 | jd = p/( 2*t1^3 + 3*t1^2*t2 + t1*t2^2 ); 87 | end 88 | if abs(t2)0 93 | t2 = ceil(t2/Ts)*Ts; 94 | jd = v/( t1^2 + t1*t2 ); 95 | end 96 | end 97 | if abs(t2)0 103 | t3 = ceil(t3/Ts)*Ts; 104 | jd = p/( 2*t1^3 + 3*t1^2*t2 + t1*t2^2 + t1^2*t3 + t1*t2*t3 ); 105 | end 106 | if abs(t3) 8 51 | help make4 52 | return 53 | else 54 | p=abs(varargin{1}); 55 | v=abs(varargin{2}); 56 | a=abs(varargin{3}); 57 | j=abs(varargin{4}); 58 | d=abs(varargin{5}); 59 | if nargin == 5 60 | Ts=0; r=eps; s=15; 61 | elseif nargin == 6 62 | Ts=abs(varargin{6}); 63 | r=eps; s=15; 64 | elseif nargin == 7 65 | Ts=abs(varargin{6}); 66 | r=abs(varargin{7}); 67 | s=15; 68 | elseif nargin == 8 69 | Ts=abs(varargin{6}); 70 | r=abs(varargin{7}); 71 | s=abs(varargin{8}); 72 | end 73 | end 74 | 75 | if isempty(p) || isempty(v) || isempty(a) || isempty(j) || isempty(d) || ... 76 | isempty(Ts) || isempty(r) || isempty(s) 77 | disp('ERROR: insufficient input for trajectory calculation') 78 | return 79 | end 80 | 81 | tol = eps; % tolerance required for continuous time calculations 82 | dd = d; % required for discrete time calculations 83 | 84 | % Calculation constant djerk phase duration: t1 85 | t1 = (1/8*p/d)^(1/4) ; % largest t1 with bound on derivative of jerk 86 | if Ts>0 87 | t1 = ceil(t1/Ts)*Ts; 88 | dd = 1/8*p/(t1^4); 89 | end 90 | % velocity test 91 | if v < 2*dd*t1^3 % v bound violated ? 92 | t1 = (1/2*v/d)^(1/3) ; % t1 with bound on velocity not violated 93 | if Ts>0 94 | t1 = ceil(t1/Ts)*Ts; 95 | dd = 1/2*v/(t1^3); 96 | end 97 | end 98 | % acceleration test 99 | if a < dd*t1^2 % a bound violated ? 100 | t1 = (a/d)^(1/2) ; % t1 with bound on acceleration not violated 101 | if Ts>0 102 | t1 = ceil(t1/Ts)*Ts; 103 | dd = a/(t1^2); 104 | end 105 | end 106 | % jerk test 107 | if j < dd*t1 % j bound violated ? 108 | t1 = j/d ; % t1 with bound on jerk not violated 109 | if Ts>0 110 | t1 = ceil(t1/Ts)*Ts; 111 | dd = j/t1; 112 | end 113 | end 114 | d = dd; % as t1 is now fixed, dd is the new bound on derivative of jerk 115 | 116 | % Calculation constant jerk phase duration: t2 117 | P = -1/9 * t1^2; % calculations to determine 118 | Q = -1/27 * t1^3 - p/(4*d*t1); % positive real solution of 119 | D = P^3 + Q^2; % third order polynomial... 120 | R = ( -Q + sqrt(D) )^(1/3); % 121 | t2 = R - P/R - 5/3*t1 ; % largest t2 with bound on jerk 122 | if Ts>0 123 | t2 = ceil(t2/Ts)*Ts; 124 | dd = p/( 8*t1^4 + 16*t1^3*t2 + 10*t1^2*t2^2 + 2*t1*t2^3 ); 125 | end 126 | if abs(t2)0 131 | t2 = ceil(t2/Ts)*Ts; 132 | dd = v/( 2*t1^3 + 3*t1^2*t2 + t1*t2^2 ); 133 | end 134 | end 135 | if abs(t2)0 140 | t2 = ceil(t2/Ts)*Ts; 141 | dd = a/( t1^2 + t1*t2 ); 142 | end 143 | end 144 | if abs(t2)0 153 | t3 = ceil(t3/Ts)*Ts; 154 | dd = p/( c1*t3^2 + c2*t3 + c3 ); 155 | end 156 | if abs(t3)0 161 | t3 = ceil(t3/Ts)*Ts; 162 | dd = v/( 2*t1^3 + 3*t1^2*t2 + t1*t2^2 + t1^2*t3 + t1*t2*t3 ); 163 | end 164 | end 165 | if abs(t3)0 171 | t4 = ceil(t4/Ts)*Ts; 172 | dd = p/( c1*t3^2 + c2*t3 + c3 + t4*(2*t1^3 + 3*t1^2*t2 + t1*t2^2 + t1^2*t3 + t1*t2*t3) ) ; 173 | end 174 | if abs(t4)0 186 | x=ceil(log10(dd)); % determine exponent of dd 187 | ddq=dd/10^x; % scale to 0-1 188 | ddq=round(ddq*10^s)/10^s; % round to s decimals 189 | ddq=ddq*10^x; 190 | % actual displacement obtained with quantized dd 191 | pp = ddq*( c1*t3^2 + c2*t3 + c3 + t4*(2*t1^3 + 3*t1^2*t2 + t1*t2^2 + t1^2*t3 + t1*t2*t3) ) ; 192 | dif=p-pp; % position error due to quantization of dd 193 | cnt=round(dif/r); % divided by resolution gives 'number of increments' 194 | % of required position correction 195 | % smooth correction obtained by dividing over entire trajectory duration 196 | tt = 8*t(1)+4*t(2)+2*t(3)+t(4); 197 | ti = tt/Ts; % should be integer number of samples 198 | cor1=sign(cnt)*floor(abs(cnt/ti))*ti; % we need cor1/ti increments correction at each 199 | % ... sample during trajectory 200 | cor2=cnt-cor1; % remaining correction: 1 increment per sample 201 | % ... during first part of trajectory 202 | dd=[ddq cor1 cor2 dd]; 203 | else 204 | dd=[dd 0 0 dd]; % continuous time result in same format 205 | end 206 | 207 | % Finished. 208 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 209 | -------------------------------------------------------------------------------- /traj/make4_it.m: -------------------------------------------------------------------------------- 1 | function [t,dd]=make4_it(varargin) 2 | 3 | % [t,dd] = make4(p,v,a,j,d,Ts,r,s) 4 | % 5 | % Calculate timing for symmetrical 4th order profiles. 6 | % This is an obsolete routine using iterations to determine solution of 3d 7 | % order polynomial equation 8 | % 9 | % inputs: 10 | % p = desired path (specify positive) [m] 11 | % v = velocity bound (specify positive) [m/s] 12 | % a = acceleration bound (specify positive) [m/s2] 13 | % j = jerk bound (specify positive) [m/s3] 14 | % d = derivative of jerk bound (specify positive) [m/s4] 15 | % Ts = sampling time [s] (optional, if not specified or 0: continuous time) 16 | % r = position resolution [m] (optional, if not specified: 10*eps) 17 | % s = number of decimals for digitized 18 | % derivative of jerk bound (optional, if not specified: 15) 19 | % 20 | % outputs: 21 | % t(1) = constant djerk phase duration 22 | % t(2) = constant jerk phase duration 23 | % t(3) = constant acceleration phase duration 24 | % t(4) = constant velocity phase duration 25 | % 26 | % t1 t1 t1 t1 27 | % .-. .-. .-. .-. 28 | % | | | | | | | | 29 | % | |t2 t3 t2| | t4 t2| | t3 | |t2 30 | % '-'--.-.----.-.--' '---------.-.--'-'----'-'--.-.-- 31 | % | | | | | | | | 32 | % | | | | | | | | 33 | % '-' '-' '-' '-' 34 | % t1 t1 t1 t1 35 | % 36 | % In case of discrete time, derivative of jerk bound d is reduced to dd and 37 | % quantized to ddq using position resolution r and number of significant decimals s 38 | % Two position correction terms are calculated to 'repair' the position error 39 | % resulting from using ddq instead of dd: 40 | % cor1 gives the number of position increments that can equally be divided 41 | % over the entire trajectory duration 42 | % cor2 gives the remaining number of position increments 43 | % The result is given as: 44 | % dd = [ ddq cor1 cor2 dd ] 45 | % 46 | 47 | % 48 | % Copyright 2004, Paul Lambrechts, The MathWorks, Inc. 49 | % 50 | 51 | % Checking validity of inputs 52 | if nargin < 5 || nargin > 8 53 | help make4 54 | return 55 | else 56 | p=abs(varargin{1}); 57 | v=abs(varargin{2}); 58 | a=abs(varargin{3}); 59 | j=abs(varargin{4}); 60 | d=abs(varargin{5}); 61 | if nargin == 5 62 | Ts=0; r=eps; s=15; 63 | elseif nargin == 6 64 | Ts=abs(varargin{6}); 65 | r=eps; s=15; 66 | elseif nargin == 7 67 | Ts=abs(varargin{6}); 68 | r=abs(varargin{7}); 69 | s=15; 70 | elseif nargin == 8 71 | Ts=abs(varargin{6}); 72 | r=abs(varargin{7}); 73 | s=abs(varargin{8}); 74 | end 75 | end 76 | 77 | if isempty(p) || isempty(v) || isempty(a) || isempty(j) || isempty(d) || ... 78 | isempty(Ts) || isempty(r) || isempty(s) 79 | disp('ERROR: insufficient input for trajectory calculation') 80 | return 81 | end 82 | 83 | tol = eps; % tolerance required for continuous time calculations 84 | dd = d; % required for discrete time calculations 85 | 86 | % Calculation constant djerk phase duration: t1 87 | t1 = (1/8*p/d)^(1/4) ; % largest t1 with bound on derivative of jerk 88 | if Ts>0 89 | t1 = ceil(t1/Ts)*Ts; 90 | dd = 1/8*p/(t1^4); 91 | end 92 | % velocity test 93 | if v < 2*dd*t1^3 % v bound violated ? 94 | t1 = (1/2*v/d)^(1/3) ; % t1 with bound on velocity not violated 95 | if Ts>0 96 | t1 = ceil(t1/Ts)*Ts; 97 | dd = 1/2*v/(t1^3); 98 | end 99 | end 100 | % acceleration test 101 | if a < dd*t1^2 % a bound violated ? 102 | t1 = (a/d)^(1/2) ; % t1 with bound on acceleration not violated 103 | if Ts>0 104 | t1 = ceil(t1/Ts)*Ts; 105 | dd = a/(t1^2); 106 | end 107 | end 108 | % jerk test 109 | if j < dd*t1 % j bound violated ? 110 | t1 = j/d ; % t1 with bound on jerk not violated 111 | if Ts>0 112 | t1 = ceil(t1/Ts)*Ts; 113 | dd = j/t1; 114 | end 115 | end 116 | d = dd; % as t1 is now fixed, dd is the new bound on derivative of jerk 117 | 118 | % Calculation constant jerk phase duration: t2 119 | t2=solve3(p,d,t1,tol); % largest t2 with bound on jerk (see function below) 120 | if Ts>0 121 | t2 = ceil(t2/Ts)*Ts; 122 | dd = p/( 8*t1^4 + 16*t1^3*t2 + 10*t1^2*t2^2 + 2*t1*t2^3 ); 123 | end 124 | if abs(t2)0 129 | t2 = ceil(t2/Ts)*Ts; 130 | dd = v/( 2*t1^3 + 3*t1^2*t2 + t1*t2^2 ); 131 | end 132 | end 133 | if abs(t2)0 138 | t2 = ceil(t2/Ts)*Ts; 139 | dd = a/( t1^2 + t1*t2 ); 140 | end 141 | end 142 | if abs(t2)0 151 | t3 = ceil(t3/Ts)*Ts; 152 | dd = p/( c1*t3^2 + c2*t3 + c3 ); 153 | end 154 | if abs(t3)0 159 | t3 = ceil(t3/Ts)*Ts; 160 | dd = v/( 2*t1^3 + 3*t1^2*t2 + t1*t2^2 + t1^2*t3 + t1*t2*t3 ); 161 | end 162 | end 163 | if abs(t3)0 169 | t4 = ceil(t4/Ts)*Ts; 170 | dd = p/( c1*t3^2 + c2*t3 + c3 + t4*(2*t1^3 + 3*t1^2*t2 + t1*t2^2 + t1^2*t3 + t1*t2*t3) ) ; 171 | end 172 | if abs(t4)0 184 | x=ceil(log10(dd)); % determine exponent of dd 185 | ddq=dd/10^x; % scale to 0-1 186 | ddq=round(ddq*10^s)/10^s; % round to s decimals 187 | ddq=ddq*10^x; 188 | % actual displacement obtained with quantized dd 189 | pp = ddq*( c1*t3^2 + c2*t3 + c3 + t4*(2*t1^3 + 3*t1^2*t2 + t1*t2^2 + t1^2*t3 + t1*t2*t3) ) ; 190 | dif=p-pp; % position error due to quantization of dd 191 | cnt=round(dif/r); % divided by resolution gives 'number of increments' 192 | % of required position correction 193 | % smooth correction obtained by dividing over entire trajectory duration 194 | tt = 8*t(1)+4*t(2)+2*t(3)+t(4); 195 | ti = tt/Ts; % should be integer number of samples 196 | cor1=sign(cnt)*floor(abs(cnt/ti))*ti; % we need cor1/ti increments correction at each 197 | % ... sample during trajectory 198 | cor2=cnt-cor1; % remaining correction: 1 increment per sample 199 | % ... during first part of trajectory 200 | dd=[ddq cor1 cor2 dd]; 201 | else 202 | dd=[dd 0 0 dd]; % continuous time result in same format 203 | end 204 | 205 | % Finished. 206 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 207 | 208 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 209 | function t2=solve3(p,d,t1,tol) 210 | % 211 | % Solve third order equation for t2: 212 | % 213 | % t2^3 + (5*t1)*t2^2 + (8*t1^2)*t2 + (4*t1^3 - p/(2*d*t1)) 214 | % 215 | % inputs: 216 | % p = desired path 217 | % d = derivative of jerk bound 218 | % t1 = derivative of jerk time interval 219 | % tol = tolerance for t2 220 | % 221 | % outputs: 222 | % t2 = constant jerk time interval 223 | % 224 | 225 | a = 4*t1^3 - p/(2*d*t1) ; % must be <= 0 226 | b = 8*t1^2 ; % must be > 0 227 | c = 5*t1 ; % must be > 0 228 | if a>eps % if a>0 t1 is calculated wrongly (eps for numerical accuracy) 229 | disp('ERROR: wrong input') 230 | keyboard 231 | return 232 | end 233 | if abs(a)= 0 and result >= 0 242 | thig = min(thig); % upper bound 243 | yhig = thig^3 + c*thig^2 + b*thig + a; % . . . because yhig positive 244 | 245 | if thig > 0 246 | i=0; % counter to prevent infinite loop 247 | while ok ~= 1 248 | tnew = tlow + (tlow-thig)/(yhig-ylow)*ylow; 249 | % tnew = (tlow + thig)/2; 250 | ynew = tnew^3 + c*tnew^2 + b*tnew + a; 251 | if abs(ynew)>tol && i<100 252 | i=i+1; 253 | tlow=tnew;ylow=ynew; % ynew always <0 due to b,c positive 254 | % (monotonously increasing derivatives) 255 | else 256 | ok=1; 257 | if i==100 258 | disp('WARNING: accuracy not reached in maximal number of iterations'); 259 | else 260 | %disp(sprintf('Third order polynomial equation solved for positive real value')); 261 | %disp(sprintf('in %i iterations, with accuracy %g',i,abs(ynew))); 262 | end 263 | end 264 | end 265 | else 266 | tnew=0; 267 | end 268 | 269 | t2=tnew; 270 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 271 | -------------------------------------------------------------------------------- /traj/model_controller.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DaiLuyaoME/Auto-tuning-of-model-based-feedforward-controller-in-ultraprecision-motion-systems/05efbdbd0e853b2d2ef3a15581306367d41388a3/traj/model_controller.slx -------------------------------------------------------------------------------- /traj/plotTraj.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DaiLuyaoME/Auto-tuning-of-model-based-feedforward-controller-in-ultraprecision-motion-systems/05efbdbd0e853b2d2ef3a15581306367d41388a3/traj/plotTraj.m -------------------------------------------------------------------------------- /traj/profile3.m: -------------------------------------------------------------------------------- 1 | function [jj,tx,j,a,v,p,tt]=profile3(t,j,acc,plt) 2 | 3 | % function [jj,tx,j,a,v,p,tt]=profile3(t,j,acc) 4 | % 5 | % Calculate symmetrical third order profiles from times: 6 | % 7 | % Inputs: 8 | % 9 | % t(1) = constant jerk phase duration 10 | % t(2) = constant acceleration phase duration (default 0) 11 | % t(3) = constant velocity phase duration (default 0) 12 | % 13 | % j = bound on jerk 14 | % acc = continuous time: accuracy for profiles: t(1)*acc = minimal timestep 15 | % discrete time: sample time 16 | % 17 | % Outputs: 18 | % 19 | % jj = derivative of jerk profile suitable for simulink 20 | % 21 | % tx = time sequence for plotting profiles 22 | % j = jerk profile 23 | % a = acceleration profile 24 | % v = velocity profile 25 | % p = position profile 26 | % 27 | % tt = 8 switching times for profile: 28 | % 29 | % 0 1 6 7 30 | % .-. .-. 31 | % | | | | 32 | % | | 2 3 4 5 | | 33 | % '-'--.-.----.-.--' '-- 34 | % | | | | 35 | % | | | | 36 | % '-' '-' 37 | % 38 | % Note: coinciding switching times are not removed 39 | 40 | % 41 | % Copyright 2004, Paul Lambrechts, The MathWorks, Inc. 42 | % 43 | 44 | if nargin < 3 || nargin > 4 45 | help profile3 46 | return 47 | end 48 | if nargin==3 49 | plt=1; 50 | end 51 | 52 | if length(t)==1 % min distance with max jerk 53 | tt= [0 1 1 2 2 3 3 4 ]*t; 54 | 55 | elseif length(t)==2 % constant acceleration phase 56 | tt= [0 1 1 2 2 3 3 4 ]*t(1) ... 57 | + [0 0 1 1 1 1 2 2 ]*t(2); 58 | 59 | elseif length(t)==3 % constant velocity phase 60 | tt= [0 1 1 2 2 3 3 4 ]*t(1) ... 61 | + [0 0 1 1 1 1 2 2 ]*t(2) ... 62 | + [0 0 0 0 1 1 1 1 ]*t(3) ; 63 | else 64 | return 65 | end 66 | 67 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 68 | % Generate Simulink look-up table 69 | jt=[]; 70 | for i=1:8 71 | jt = [jt [1 1] * tt(i) ] ; 72 | end 73 | jt = [jt 1.5*tt(8)]; 74 | 75 | jj = [jt ; [ 0 j j 0 0 -j -j 0 0 -j -j 0 0 j j 0 0 ] ]; 76 | 77 | if plt==0 % no plot required 78 | tx=[];j=[];a=[];v=[];p=[]; % dummy outputs 79 | return 80 | end 81 | 82 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 83 | % Generate profiles for plotting 84 | 85 | % Determine continuous or discrete 86 | if max(abs( round(t/acc)-t/acc )) > 1e-12 % continuous 87 | 88 | disp('Calculating continuous time profiles') 89 | step = t(1)*acc; 90 | tx=0:step:1.2*tt(8); 91 | x=[]; 92 | for i=0:step:1.2*tt(8) 93 | j=find(i<=jj(1,:)); 94 | x=[x ; jj(2,j(1))]; 95 | end 96 | j=x; 97 | a=cumsum(j)*step; 98 | v=cumsum(a)*step; 99 | p=cumsum(v)*step; 100 | 101 | else % discrete 102 | 103 | disp('Calculating discrete time profiles') 104 | Ts=acc; 105 | ttest=[tt 1.5*tt(8)]; 106 | len = round(1.2*tt(8)/Ts + 1); % length of profiles 107 | xj = zeros(len,1); 108 | xa = xj; 109 | xv = xj; 110 | xp = xj; 111 | xj(1) = j; 112 | tx=0:Ts:1.2*tt(8)+Ts/2; 113 | for time=Ts:Ts:(1.2*tt(8)+Ts/2) 114 | i = find( (time + Ts/2) <= ttest ); i = i(1)-1; 115 | k = round(time/Ts); 116 | if i==1 || i==7 117 | xj(k+1) = j; 118 | elseif i==3 || i==5 119 | xj(k+1) = -j; 120 | else 121 | xj(k+1) = 0; 122 | end 123 | xa(k+1) = xa(k) + xj(k)*Ts; 124 | xv(k+1) = xv(k) + xa(k)*Ts; 125 | xp(k+1) = xp(k) + xv(k)*Ts; 126 | end 127 | j=xj;a=xa;v=xv;p=xp; 128 | 129 | end 130 | 131 | %close all 132 | %figure 133 | subplot(411);plot(tx,j,'k','LineWidth',1.5);hold on;plot([0 0],[-1 1]*max(j),'k--',[1 1]*max(tt),[-1 1]*max(j),'k--','LineWidth',1.5);grid on; axis([ [-0.01 1]*max(tx) [-1.1 1.1]*max(j)]); 134 | title('Third order trajectory profiles');ylabel('j [m/s3]'); 135 | subplot(412);plot(tx,a,'k','LineWidth',1.5);hold on;plot([0 0],[-1 1]*max(a),'k--',[1 1]*max(tt),[-1 1]*max(a),'k--','LineWidth',1.5);grid on; axis([ [-0.01 1]*max(tx) [-1.1 1.1]*max(a)]); 136 | ylabel('a [m/s2]'); 137 | subplot(413);plot(tx,v,'k','LineWidth',1.5);hold on;plot([0 0],[0 1]*max(v),'k--',[1 1]*max(tt),[0 1]*max(v),'k--','LineWidth',1.5);grid on; axis([ [-0.01 1]*max(tx) [-0.1 1.1]*max(v)]); 138 | ylabel('v [m/s]'); 139 | subplot(414);plot(tx,p,'k','LineWidth',1.5);hold on;plot([0 0],[0 1]*max(p),'k--',[1 1]*max(tt),[0 1]*max(p),'k--','LineWidth',1.5);grid on; axis([ [-0.01 1]*max(tx) [-0.1 1.1]*max(p)]); 140 | xlabel('time [s]');ylabel('x [m]'); 141 | set(1,'position',[700 200 500 680]) 142 | set(1,'paperposition',[0 0 5 6.8]) 143 | 144 | subplot(411); 145 | text(tt(1)+tt(8)/200,max(j)/5,'t_0'); 146 | text(tt(2)+tt(8)/200,max(j)/5,'t_1'); 147 | text(tt(3) ,max(j)/5,'t_2'); 148 | text(tt(4) ,max(j)/5,'t_3'); 149 | text(tt(5) ,max(j)/5,'t_4'); 150 | text(tt(6) ,max(j)/5,'t_5'); 151 | text(tt(7)+tt(8)/200,max(j)/5,'t_6'); 152 | text(tt(8)+tt(8)/200,max(j)/5,'t_7'); 153 | -------------------------------------------------------------------------------- /traj/profile4.m: -------------------------------------------------------------------------------- 1 | function [dj,tx,d,j,a,v,p,tt]=profile4(t,d,acc,plt) 2 | 3 | % function [dj,tx,d,j,a,v,p,tt]=profile4(t,d,acc) 4 | % 5 | % Calculate symmetrical fourth order profiles from times: 6 | % 7 | % Inputs: 8 | % 9 | % t(1) = constant djerk phase duration 10 | % t(2) = constant jerk phase duration (default 0) 11 | % t(3) = constant acceleration phase duration (default 0) 12 | % t(4) = constant velocity phase duration (default 0) 13 | % 14 | % d = bound on djerk 15 | % acc = continuous time: accuracy for profiles: t(1)*acc = minimal timestep 16 | % discrete time: sample time 17 | % 18 | % Outputs: 19 | % 20 | % dj = derivative of jerk profile suitable for simulink 21 | % 22 | % tx = time sequence for plotting profiles 23 | % d = derivative of jerk profile 24 | % j = jerk profile 25 | % a = acceleration profile 26 | % v = velocity profile 27 | % p = position profile 28 | % 29 | % tt = 16 switching times for profile: 30 | % 31 | % 0 1 6 7 10 11 12 13 32 | % .-. .-. .-. .-. 33 | % | | | | | | | | 34 | % | | 2 3 4 5 | | 8 9 | | | | 14 15 35 | % '-'--.-.----.-.--' '---------.-.--'-'----'-'--.-.-- 36 | % | | | | | | | | 37 | % | | | | | | | | 38 | % '-' '-' '-' '-' 39 | % 40 | % Note: coinciding switching times are not removed 41 | 42 | % 43 | % Copyright 2004, Paul Lambrechts, The MathWorks, Inc. 44 | % 45 | 46 | if nargin~=3 && nargin~=4 47 | help profile4 48 | return 49 | end 50 | if nargin==3 51 | plt=1; 52 | end 53 | 54 | 55 | if length(t)==1 % min distance with max djerk 56 | tt= [0 1 1 2 2 3 3 4 4 5 5 6 6 7 7 8]*t; 57 | % tt= [0 1 1 2 2 3 4 4 4 4 4 5 5 6 6 7]*t; 58 | 59 | elseif length(t)==2 % constant jerk phase 60 | tt= [0 1 1 2 2 3 3 4 4 5 5 6 6 7 7 8]*t(1) ... 61 | + [0 0 1 1 1 1 2 2 2 2 3 3 3 3 4 4]*t(2); 62 | 63 | elseif length(t)==3 % constant acceleration phase 64 | tt= [0 1 1 2 2 3 3 4 4 5 5 6 6 7 7 8]*t(1) ... 65 | + [0 0 1 1 1 1 2 2 2 2 3 3 3 3 4 4]*t(2) ... 66 | + [0 0 0 0 1 1 1 1 1 1 1 1 2 2 2 2]*t(3) ; 67 | 68 | elseif length(t)==4 % constant velocity phase 69 | tt= [0 1 1 2 2 3 3 4 4 5 5 6 6 7 7 8]*t(1) ... 70 | + [0 0 1 1 1 1 2 2 2 2 3 3 3 3 4 4]*t(2) ... 71 | + [0 0 0 0 1 1 1 1 1 1 1 1 2 2 2 2]*t(3) ... 72 | + [0 0 0 0 0 0 0 0 1 1 1 1 1 1 1 1]*t(4) ; 73 | 74 | else 75 | return 76 | end 77 | 78 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 79 | % Generate Simulink look-up table 80 | dt=[]; 81 | for i=1:16 82 | dt = [dt [1 1]*tt(i) ]; 83 | end 84 | dt = [dt 1.5*tt(16)]; 85 | 86 | dd = [0 d d 0]; 87 | dd = [ dd -dd -dd dd -dd dd dd -dd 0]; 88 | 89 | dj = [dt ; dd] ; 90 | 91 | if plt==0 % no plot required 92 | tx=[];d=[];j=[];a=[];v=[];p=[]; % dummy outputs 93 | return 94 | end 95 | 96 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 97 | % Generate profiles for plotting 98 | 99 | % Determine continuous or discrete 100 | if max(abs( round(t/acc)-t/acc )) > 1e-12 % continuous 101 | disp('Calculating continuous time profiles') 102 | step = t(1)*acc; 103 | tx=(0:step:1.2*tt(16))'; 104 | x=[]; 105 | for i=0:step:1.2*tt(16) 106 | j=find(i<=dj(1,:)); 107 | x=[x ; dj(2,j(1))]; 108 | end 109 | d=x; 110 | j=cumsum(d)*step; 111 | a=cumsum(j)*step; 112 | v=cumsum(a)*step; 113 | p=cumsum(v)*step; 114 | 115 | else % discrete 116 | 117 | disp('Calculating discrete time profiles') 118 | Ts=acc; 119 | ttest=[tt 1.5*tt(16)]; 120 | len = round(1.2*tt(16)/Ts + 1); % length of profiles 121 | xd = zeros(len,1); 122 | xj = xd; 123 | xa = xd; 124 | xv = xd; 125 | xp = xd; 126 | xd(1) = d; 127 | tx=(0:Ts:1.2*tt(16)+Ts/2)'; 128 | for time=Ts:Ts:1.2*tt(16)+Ts/2 129 | j = find( (time + Ts/2) <= ttest ); j = j(1)-1; 130 | k = round(time/Ts); 131 | if j==1 || j==7 || j==11 || j==13 132 | xd(k+1) = d; 133 | elseif j==3 || j==5 || j==9 || j==15 134 | xd(k+1) = -d; 135 | else 136 | xd(k+1) = 0; 137 | end 138 | xj(k+1) = xj(k) + xd(k)*Ts; 139 | xa(k+1) = xa(k) + xj(k)*Ts; 140 | xv(k+1) = xv(k) + xa(k)*Ts; 141 | xp(k+1) = xp(k) + xv(k)*Ts; 142 | end 143 | d=xd;j=xj;a=xa;v=xv;p=xp; 144 | 145 | dj(1,:)=dj(1,:)+Ts/2 ; % add Ts/2 to avoid numerical problems 146 | end 147 | dj=dj'; % to be compatible with Simulink 'From Workspace' block 148 | 149 | % figure 150 | %close all 151 | subplot(511);plot(tx,d,'k','LineWidth',1.5);hold on;plot([0 0],[-1 1]*max(d),'k--',[1 1]*max(tt),[-1 1]*max(d),'k--','LineWidth',1.5); grid on; axis([ [-0.01 1]*max(tx) [-1.1 1.1]*max(d)]); 152 | title('Fourth order trajectory profiles');ylabel('d [m/s4]'); 153 | subplot(512);plot(tx,j,'k','LineWidth',1.5);hold on;plot([0 0],[-1 1]*max(j),'k--',[1 1]*max(tt),[-1 1]*max(j),'k--','LineWidth',1.5);grid on; axis([ [-0.01 1]*max(tx) [-1.1 1.1]*max(j)]); 154 | ylabel('j [m/s3]'); 155 | subplot(513);plot(tx,a,'k','LineWidth',1.5);hold on;plot([0 0],[-1 1]*max(a),'k--',[1 1]*max(tt),[-1 1]*max(a),'k--','LineWidth',1.5);grid on; axis([ [-0.01 1]*max(tx) [-1.1 1.1]*max(a)]); 156 | ylabel('a [m/s2]'); 157 | subplot(514);plot(tx,v,'k','LineWidth',1.5);hold on;plot([0 0],[0 1]*max(v),'k--',[1 1]*max(tt),[0 1]*max(v),'k--','LineWidth',1.5);grid on; axis([ [-0.01 1]*max(tx) [-0.1 1.1]*max(v)]); 158 | ylabel('v [m/s]'); 159 | subplot(515);plot(tx,p,'k','LineWidth',1.5);hold on;plot([0 0],[0 1]*max(p),'k--',[1 1]*max(tt),[0 1]*max(p),'k--','LineWidth',1.5);grid on; axis([ [-0.01 1]*max(tx) [-0.1 1.1]*max(p)]); 160 | xlabel('time [s]');ylabel('x [m]'); 161 | set(1,'position',[700 100 500 860]) 162 | set(1,'paperposition',[0 0 5 8.6]) 163 | 164 | return 165 | subplot(511); 166 | text(tt( 1)+tt(16)/200,-max(d)/5,'t_0'); 167 | text(tt( 2)-tt(16)/200,-max(d)/5,'t_1'); 168 | text(tt( 3)-tt(16)/200, max(d)/5,'t_2'); 169 | text(tt( 4)-tt(16)/200, max(d)/5,'t_3'); 170 | text(tt( 5)-tt(16)/200, max(d)/5,'t_4'); 171 | text(tt( 6)-tt(16)/200, max(d)/5,'t_5'); 172 | text(tt( 7)-tt(16)/200,-max(d)/5,'t_6'); 173 | text(tt( 8)-tt(16)/200,-max(d)/5,'t_7'); 174 | text(tt( 9)-tt(16)/200, max(d)/5,'t_8'); 175 | text(tt(10)-tt(16)/200, max(d)/5,'t_9'); 176 | text(tt(11)-tt(16)/100,-max(d)/5,'t_1_0'); 177 | text(tt(12)-tt(16)/200,-max(d)/5,'t_1_1'); 178 | text(tt(13)-tt(16)/100,-max(d)/5,'t_1_2'); 179 | text(tt(14)-tt(16)/200,-max(d)/5,'t_1_3'); 180 | text(tt(15)-tt(16)/100, max(d)/5,'t_1_4'); 181 | text(tt(16)-tt(16)/200, max(d)/5,'t_1_5'); 182 | -------------------------------------------------------------------------------- /traj/test.m: -------------------------------------------------------------------------------- 1 | plotTraj(acc.time,1000 * dis.signals.values,1000 * vel.signals.values,acc.signals.values,jerk.signals.values,snap.signals.values); 2 | %% 3 | plotTraj(acc.time,1000 * dis.signals.values,1000 * vel.signals.values,acc.signals.values,jerk.signals.values,snap.signals.values) -------------------------------------------------------------------------------- /traj/trajPowerSpectralAnalysis.m: -------------------------------------------------------------------------------- 1 | figure; 2 | powerSpectralAnalysis(traj,5000); 3 | figure; 4 | cumulativePowerSpectral(traj,5000,1,1); 5 | %% 6 | global accStartTime; 7 | global accEndTime; 8 | -------------------------------------------------------------------------------- /traj/trajTest.mdl.r2017a: -------------------------------------------------------------------------------- 1 | Model { 2 | Name "trajTest" 3 | Version 8.9 4 | SavedCharacterEncoding "GBK" 5 | GraphicalInterface { 6 | NumRootInports 0 7 | NumRootOutports 0 8 | ParameterArgumentNames "" 9 | ComputedModelVersion "1.14" 10 | NumModelReferences 0 11 | NumTestPointedSignals 5 12 | TestPointedSignal { 13 | SignalName "snap" 14 | FullBlockPath "trajTest/Demux1" 15 | LogSignal 1 16 | MaxPoints 5000 17 | Decimation 2 18 | } 19 | TestPointedSignal { 20 | SignalName "jerk" 21 | FullBlockPath "trajTest/Demux1" 22 | PortIndex 2 23 | LogSignal 1 24 | MaxPoints 5000 25 | Decimation 2 26 | } 27 | TestPointedSignal { 28 | SignalName "acc" 29 | FullBlockPath "trajTest/Demux1" 30 | PortIndex 3 31 | LogSignal 1 32 | MaxPoints 5000 33 | Decimation 2 34 | } 35 | TestPointedSignal { 36 | SignalName "vel" 37 | FullBlockPath "trajTest/Demux1" 38 | PortIndex 4 39 | LogSignal 1 40 | MaxPoints 5000 41 | Decimation 2 42 | } 43 | TestPointedSignal { 44 | SignalName "dis" 45 | FullBlockPath "trajTest/Demux1" 46 | PortIndex 5 47 | LogSignal 1 48 | MaxPoints 5000 49 | Decimation 2 50 | } 51 | NumProvidedFunctions 0 52 | NumRequiredFunctions 0 53 | NumResetEvents 0 54 | HasInitializeEvent 0 55 | HasTerminateEvent 0 56 | IsExportFunctionModel 0 57 | NumParameterArguments 0 58 | OrderedModelArguments 1 59 | } 60 | LogicAnalyzerGraphicalSettings "" 61 | LogicAnalyzerPlugin "on" 62 | LogicAnalyzerSignalOrdering "" 63 | DiagnosticSuppressor "on" 64 | SuppressorTable "22 serialization::archive 11 0 6 0 0 0 8 0" 65 | SLCCPlugin "on" 66 | slprops.hdlmdlprops { 67 | $PropName "HDLParams" 68 | $ObjectID 1 69 | Array { 70 | Type "Cell" 71 | Dimension 2 72 | Cell "HDLSubsystem" 73 | Cell "$bdroot" 74 | PropName "mdlProps" 75 | } 76 | } 77 | ScopeRefreshTime 0.035000 78 | OverrideScopeRefreshTime on 79 | DisableAllScopes off 80 | DataTypeOverride "UseLocalSettings" 81 | DataTypeOverrideAppliesTo "AllNumericTypes" 82 | MinMaxOverflowLogging "UseLocalSettings" 83 | MinMaxOverflowArchiveMode "Overwrite" 84 | FPTRunName "Run 1" 85 | MaxMDLFileLineLength 120 86 | LastSavedArchitecture "win64" 87 | Object { 88 | $PropName "BdWindowsInfo" 89 | $ObjectID 2 90 | $ClassName "Simulink.BDWindowsInfo" 91 | Object { 92 | $PropName "WindowsInfo" 93 | $ObjectID 3 94 | $ClassName "Simulink.WindowInfo" 95 | IsActive [1] 96 | Location [8.0, 8.0, 1550.0, 838.0] 97 | Object { 98 | $PropName "ModelBrowserInfo" 99 | $ObjectID 4 100 | $ClassName "Simulink.ModelBrowserInfo" 101 | Visible [0] 102 | DockPosition "Left" 103 | Width [50] 104 | Height [50] 105 | Filter [9] 106 | } 107 | Object { 108 | $PropName "ExplorerBarInfo" 109 | $ObjectID 5 110 | $ClassName "Simulink.ExplorerBarInfo" 111 | Visible [1] 112 | } 113 | Object { 114 | $PropName "EditorsInfo" 115 | $ObjectID 6 116 | $ClassName "Simulink.EditorInfo" 117 | IsActive [1] 118 | ViewObjType "SimulinkTopLevel" 119 | LoadSaveID "0" 120 | Extents [1875.0, 835.0] 121 | ZoomFactor [1.0] 122 | Offset [-221.08952380952348, -57.966428493942544] 123 | } 124 | Object { 125 | $PropName "DockComponentsInfo" 126 | $ObjectID 7 127 | $ClassName "Simulink.DockComponentInfo" 128 | Type "GLUE2:PropertyInspector" 129 | ID "Property Inspector" 130 | Visible [0] 131 | CreateCallback "" 132 | UserData "" 133 | Floating [0] 134 | DockPosition "Right" 135 | Width [512] 136 | Height [384] 137 | } 138 | WindowState "AAAA/wAAAAD9AAAAAgAAAAAAAAC9AAAB+PwCAAAAA/sAAAAWAEQAbwBjAGsAVwBpAGQAZwBlAHQAMwEAAAAxAAAB+AAAA" 139 | "AAAAAAA+wAAABYARABvAGMAawBXAGkAZABnAGUAdAA0AAAAAAD/////AAAAAAAAAAD7AAAAUgBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0Ac" 140 | "ABvAG4AZQBuAHQALwBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0AcABvAG4AZQBuAHQAAAAAAP////8AAAB4AP///wAAAAEAAAAAAAAAAPwCA" 141 | "AAAAfsAAABUAEcATABVAEUAMgA6AFAAcgBvAHAAZQByAHQAeQBJAG4AcwBwAGUAYwB0AG8AcgAvAFAAcgBvAHAAZQByAHQAeQAgAEkAbgBzAHAAZ" 142 | "QBjAHQAbwByAAAAAAD/////AAAANQD///8AAAeAAAADiAAAAAEAAAACAAAAAQAAAAL8AAAAAQAAAAIAAAAP/////wAAAAAA/////wAAAAAAAAAA/" 143 | "////wEAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/" 144 | "////wEAAACR/////wAAAAAAAAAA/////wEAAAEE/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wEAAAGV/////wAAAAAAAAAA/" 145 | "////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wEAAAQV/////wAAAAAAAAAA/" 146 | "////wEAAARN/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA" 147 | } 148 | } 149 | HideAutomaticNames on 150 | Created "Sun Mar 26 13:52:43 2017" 151 | Creator "Dai Luyao" 152 | UpdateHistory "UpdateHistoryNever" 153 | ModifiedByFormat "%" 154 | LastModifiedBy "Dai Luyao" 155 | ModifiedDateFormat "%" 156 | LastModifiedDate "Fri Dec 29 14:03:45 2017" 157 | RTWModifiedTimeStamp 435801064 158 | ModelVersionFormat "1.%" 159 | ConfigurationManager "none" 160 | SampleTimeColors off 161 | SampleTimeAnnotations off 162 | LibraryLinkDisplay "disabled" 163 | WideLines off 164 | ShowLineDimensions off 165 | ShowPortDataTypes off 166 | PortDataTypeDisplayFormat "AliasTypeOnly" 167 | ShowEditTimeErrors on 168 | ShowEditTimeWarnings on 169 | ShowEditTimeAdvisorChecks off 170 | ShowPortUnits off 171 | ShowDesignRanges off 172 | ShowLoopsOnError on 173 | IgnoreBidirectionalLines off 174 | ShowStorageClass off 175 | ShowTestPointIcons on 176 | ShowSignalResolutionIcons on 177 | ShowViewerIcons on 178 | SortedOrder off 179 | VariantCondition off 180 | ExecutionContextIcon off 181 | ShowLinearizationAnnotations on 182 | ShowVisualizeInsertedRTB on 183 | ShowMarkup on 184 | BlockNameDataTip off 185 | BlockParametersDataTip off 186 | BlockDescriptionStringDataTip off 187 | ToolBar on 188 | StatusBar on 189 | BrowserShowLibraryLinks off 190 | FunctionConnectors off 191 | BrowserLookUnderMasks off 192 | SimulationMode "normal" 193 | VisualizeLoggedSignalsWhenLoggingToFile off 194 | PauseTimes "5" 195 | NumberOfSteps 1 196 | SnapshotBufferSize 10 197 | SnapshotInterval 10 198 | NumberOfLastSnapshots 0 199 | LinearizationMsg "none" 200 | Profile off 201 | ParamWorkspaceSource "MATLABWorkspace" 202 | AccelSystemTargetFile "accel.tlc" 203 | AccelTemplateMakefile "accel_default_tmf" 204 | AccelMakeCommand "make_rtw" 205 | TryForcingSFcnDF off 206 | Object { 207 | $PropName "DataLoggingOverride" 208 | $ObjectID 8 209 | $ClassName "Simulink.SimulationData.ModelLoggingInfo" 210 | model_ "trajTest" 211 | overrideMode_ [0U] 212 | Array { 213 | Type "Cell" 214 | Dimension 1 215 | Cell "trajTest" 216 | PropName "logAsSpecifiedByModels_" 217 | } 218 | Array { 219 | Type "Cell" 220 | Dimension 1 221 | Cell [] 222 | PropName "logAsSpecifiedByModelsSSIDs_" 223 | } 224 | } 225 | Object { 226 | $PropName "InstrumentedSignals" 227 | $ObjectID 9 228 | $ClassName "Simulink.HMI.InstrumentedSignals" 229 | Array { 230 | Type "Struct" 231 | Dimension 5 232 | MATStruct { 233 | UUID "7b0a832c-55e1-4c84-9c2f-50125079abf1" 234 | BlockPath_ "Demux1" 235 | SID_ "1" 236 | SubPath_ "" 237 | OutputPortIndex_ [1.0] 238 | LogicalPortIndex_ [0.0] 239 | SignalName_ "snap" 240 | SubSysPath_ "" 241 | Decimation_ [1.0] 242 | MaxPoints_ [1024.0] 243 | TargetBufferedStreaming_ [0.0] 244 | } 245 | MATStruct { 246 | UUID "55e9a62c-f6de-452a-a455-10227ab5348e" 247 | BlockPath_ "Demux1" 248 | SID_ "1" 249 | SubPath_ "" 250 | OutputPortIndex_ [2.0] 251 | LogicalPortIndex_ [0.0] 252 | SignalName_ "jerk" 253 | SubSysPath_ "" 254 | Decimation_ [1.0] 255 | MaxPoints_ [1024.0] 256 | TargetBufferedStreaming_ [0.0] 257 | } 258 | MATStruct { 259 | UUID "bd02ea85-a4b6-4bf6-a226-0c564ceae4a1" 260 | BlockPath_ "Demux1" 261 | SID_ "1" 262 | SubPath_ "" 263 | OutputPortIndex_ [3.0] 264 | LogicalPortIndex_ [0.0] 265 | SignalName_ "acc" 266 | SubSysPath_ "" 267 | Decimation_ [1.0] 268 | MaxPoints_ [1024.0] 269 | TargetBufferedStreaming_ [0.0] 270 | } 271 | MATStruct { 272 | UUID "c60aaa78-d664-4588-9c93-52c3be55e188" 273 | BlockPath_ "Demux1" 274 | SID_ "1" 275 | SubPath_ "" 276 | OutputPortIndex_ [4.0] 277 | LogicalPortIndex_ [0.0] 278 | SignalName_ "vel" 279 | SubSysPath_ "" 280 | Decimation_ [1.0] 281 | MaxPoints_ [1024.0] 282 | TargetBufferedStreaming_ [0.0] 283 | } 284 | MATStruct { 285 | UUID "50d53fee-38ae-4e48-9d96-f2cc94f38cb5" 286 | BlockPath_ "Demux1" 287 | SID_ "1" 288 | SubPath_ "" 289 | OutputPortIndex_ [5.0] 290 | LogicalPortIndex_ [0.0] 291 | SignalName_ "dis" 292 | SubSysPath_ "" 293 | Decimation_ [1.0] 294 | MaxPoints_ [1024.0] 295 | TargetBufferedStreaming_ [0.0] 296 | } 297 | PropName "Persistence" 298 | } 299 | } 300 | ExtModeBatchMode off 301 | ExtModeEnableFloating on 302 | ExtModeTrigType "manual" 303 | ExtModeTrigMode "normal" 304 | ExtModeTrigPort "1" 305 | ExtModeTrigElement "any" 306 | ExtModeTrigDuration 1000 307 | ExtModeTrigDurationFloating "auto" 308 | ExtModeTrigHoldOff 0 309 | ExtModeTrigDelay 0 310 | ExtModeTrigDirection "rising" 311 | ExtModeTrigLevel 0 312 | ExtModeArchiveMode "off" 313 | ExtModeAutoIncOneShot off 314 | ExtModeIncDirWhenArm off 315 | ExtModeAddSuffixToVar off 316 | ExtModeWriteAllDataToWs off 317 | ExtModeArmWhenConnect on 318 | ExtModeSkipDownloadWhenConnect off 319 | ExtModeLogAll on 320 | ExtModeAutoUpdateStatusClock on 321 | ShowModelReferenceBlockVersion off 322 | ShowModelReferenceBlockIO off 323 | OrderedModelArguments on 324 | Array { 325 | Type "Handle" 326 | Dimension 1 327 | Simulink.ConfigSet { 328 | $ObjectID 10 329 | Version "1.17.0" 330 | DisabledProps [] 331 | Description "" 332 | Array { 333 | Type "Handle" 334 | Dimension 10 335 | Simulink.SolverCC { 336 | $ObjectID 11 337 | Version "1.17.0" 338 | DisabledProps [] 339 | Description "" 340 | StartTime "0.0" 341 | StopTime "0.030" 342 | AbsTol "auto" 343 | FixedStep "5e-5" 344 | InitialStep "auto" 345 | MaxNumMinSteps "-1" 346 | MaxOrder 5 347 | ZcThreshold "auto" 348 | ConsecutiveZCsStepRelTol "10*128*eps" 349 | MaxConsecutiveZCs "1000" 350 | ExtrapolationOrder 4 351 | NumberNewtonIterations 1 352 | MaxStep "0.0002" 353 | MinStep "auto" 354 | MaxConsecutiveMinStep "1" 355 | RelTol "1e-3" 356 | EnableMultiTasking off 357 | EnableConcurrentExecution off 358 | ConcurrentTasks off 359 | Solver "VariableStepAuto" 360 | SolverName "VariableStepAuto" 361 | SolverJacobianMethodControl "auto" 362 | ShapePreserveControl "DisableAll" 363 | ZeroCrossControl "UseLocalSettings" 364 | ZeroCrossAlgorithm "Nonadaptive" 365 | AlgebraicLoopSolver "TrustRegion" 366 | SolverInfoToggleStatus on 367 | IsAutoAppliedInSIP off 368 | SolverResetMethod "Fast" 369 | PositivePriorityOrder off 370 | AutoInsertRateTranBlk off 371 | SampleTimeConstraint "Unconstrained" 372 | InsertRTBMode "Whenever possible" 373 | SampleTimeProperty [] 374 | } 375 | Simulink.DataIOCC { 376 | $ObjectID 12 377 | Version "1.17.0" 378 | DisabledProps [] 379 | Description "" 380 | Decimation "1" 381 | ExternalInput "[t, u]" 382 | FinalStateName "xFinal" 383 | InitialState "xInitial" 384 | LimitDataPoints off 385 | MaxDataPoints "1000" 386 | LoadExternalInput off 387 | LoadInitialState off 388 | SaveFinalState off 389 | SaveCompleteFinalSimState off 390 | SaveFormat "Dataset" 391 | SignalLoggingSaveFormat "Dataset" 392 | SaveOutput on 393 | SaveState off 394 | SignalLogging on 395 | DSMLogging on 396 | InspectSignalLogs off 397 | VisualizeSimOutput on 398 | StreamToWorkspace off 399 | StreamVariableName "streamout" 400 | SaveTime on 401 | ReturnWorkspaceOutputs off 402 | StateSaveName "xout" 403 | TimeSaveName "tout" 404 | OutputSaveName "yout" 405 | SignalLoggingName "logsout" 406 | DSMLoggingName "dsmout" 407 | OutputOption "RefineOutputTimes" 408 | OutputTimes "[]" 409 | ReturnWorkspaceOutputsName "out" 410 | Refine "1" 411 | LoggingToFile off 412 | DatasetSignalFormat "timeseries" 413 | LoggingFileName "out.mat" 414 | LoggingIntervals "[-inf, inf]" 415 | } 416 | Simulink.OptimizationCC { 417 | $ObjectID 13 418 | Version "1.17.0" 419 | Array { 420 | Type "Cell" 421 | Dimension 8 422 | Cell "BooleansAsBitfields" 423 | Cell "PassReuseOutputArgsAs" 424 | Cell "PassReuseOutputArgsThreshold" 425 | Cell "ZeroExternalMemoryAtStartup" 426 | Cell "ZeroInternalMemoryAtStartup" 427 | Cell "OptimizeModelRefInitCode" 428 | Cell "NoFixptDivByZeroProtection" 429 | Cell "UseSpecifiedMinMax" 430 | PropName "DisabledProps" 431 | } 432 | Description "" 433 | BlockReduction on 434 | BooleanDataType on 435 | ConditionallyExecuteInputs on 436 | DefaultParameterBehavior "Tunable" 437 | UseDivisionForNetSlopeComputation "off" 438 | UseFloatMulNetSlope off 439 | DefaultUnderspecifiedDataType "double" 440 | UseSpecifiedMinMax off 441 | InlineInvariantSignals off 442 | OptimizeBlockIOStorage on 443 | BufferReuse on 444 | EnhancedBackFolding off 445 | CachingGlobalReferences off 446 | GlobalBufferReuse on 447 | StrengthReduction off 448 | AdvancedOptControl "" 449 | ExpressionFolding on 450 | BooleansAsBitfields off 451 | BitfieldContainerType "uint_T" 452 | EnableMemcpy on 453 | MemcpyThreshold 64 454 | PassReuseOutputArgsAs "Structure reference" 455 | PassReuseOutputArgsThreshold 12 456 | ExpressionDepthLimit 128 457 | LocalBlockOutputs on 458 | RollThreshold 5 459 | StateBitsets off 460 | DataBitsets off 461 | ActiveStateOutputEnumStorageType "Native Integer" 462 | ZeroExternalMemoryAtStartup on 463 | ZeroInternalMemoryAtStartup on 464 | InitFltsAndDblsToZero off 465 | NoFixptDivByZeroProtection off 466 | EfficientFloat2IntCast off 467 | EfficientMapNaN2IntZero on 468 | LifeSpan "auto" 469 | MaxStackSize "Inherit from target" 470 | BufferReusableBoundary on 471 | SimCompilerOptimization "off" 472 | AccelVerboseBuild off 473 | OptimizeBlockOrder "off" 474 | OptimizeDataStoreBuffers on 475 | BusAssignmentInplaceUpdate on 476 | } 477 | Simulink.DebuggingCC { 478 | $ObjectID 14 479 | Version "1.17.0" 480 | Array { 481 | Type "Cell" 482 | Dimension 1 483 | Cell "UseOnlyExistingSharedCode" 484 | PropName "DisabledProps" 485 | } 486 | Description "" 487 | RTPrefix "error" 488 | ConsistencyChecking "none" 489 | ArrayBoundsChecking "none" 490 | SignalInfNanChecking "none" 491 | SignalRangeChecking "none" 492 | ReadBeforeWriteMsg "UseLocalSettings" 493 | WriteAfterWriteMsg "UseLocalSettings" 494 | WriteAfterReadMsg "UseLocalSettings" 495 | AlgebraicLoopMsg "warning" 496 | ArtificialAlgebraicLoopMsg "warning" 497 | SaveWithDisabledLinksMsg "warning" 498 | SaveWithParameterizedLinksMsg "warning" 499 | CheckSSInitialOutputMsg on 500 | UnderspecifiedInitializationDetection "Simplified" 501 | MergeDetectMultiDrivingBlocksExec "error" 502 | CheckExecutionContextPreStartOutputMsg off 503 | CheckExecutionContextRuntimeOutputMsg off 504 | SignalResolutionControl "UseLocalSettings" 505 | BlockPriorityViolationMsg "warning" 506 | MinStepSizeMsg "warning" 507 | TimeAdjustmentMsg "none" 508 | MaxConsecutiveZCsMsg "error" 509 | MaskedZcDiagnostic "warning" 510 | IgnoredZcDiagnostic "warning" 511 | SolverPrmCheckMsg "none" 512 | InheritedTsInSrcMsg "warning" 513 | MultiTaskDSMMsg "error" 514 | MultiTaskCondExecSysMsg "error" 515 | MultiTaskRateTransMsg "error" 516 | SingleTaskRateTransMsg "none" 517 | TasksWithSamePriorityMsg "warning" 518 | SigSpecEnsureSampleTimeMsg "warning" 519 | CheckMatrixSingularityMsg "none" 520 | IntegerOverflowMsg "warning" 521 | Int32ToFloatConvMsg "warning" 522 | ParameterDowncastMsg "error" 523 | ParameterOverflowMsg "error" 524 | ParameterUnderflowMsg "none" 525 | ParameterPrecisionLossMsg "warning" 526 | ParameterTunabilityLossMsg "warning" 527 | FixptConstUnderflowMsg "none" 528 | FixptConstOverflowMsg "none" 529 | FixptConstPrecisionLossMsg "none" 530 | UnderSpecifiedDataTypeMsg "none" 531 | UnnecessaryDatatypeConvMsg "none" 532 | VectorMatrixConversionMsg "none" 533 | InvalidFcnCallConnMsg "error" 534 | FcnCallInpInsideContextMsg "error" 535 | SignalLabelMismatchMsg "none" 536 | UnconnectedInputMsg "warning" 537 | UnconnectedOutputMsg "warning" 538 | UnconnectedLineMsg "warning" 539 | UseOnlyExistingSharedCode "error" 540 | SFcnCompatibilityMsg "none" 541 | FrameProcessingCompatibilityMsg "error" 542 | UniqueDataStoreMsg "none" 543 | BusObjectLabelMismatch "warning" 544 | RootOutportRequireBusObject "warning" 545 | AssertControl "UseLocalSettings" 546 | AllowSymbolicDim on 547 | RowMajorDimensionSupport off 548 | ModelReferenceIOMsg "none" 549 | ModelReferenceMultiInstanceNormalModeStructChecksumCheck "error" 550 | ModelReferenceVersionMismatchMessage "none" 551 | ModelReferenceIOMismatchMessage "none" 552 | UnknownTsInhSupMsg "warning" 553 | ModelReferenceDataLoggingMessage "warning" 554 | ModelReferenceSymbolNameMessage "warning" 555 | ModelReferenceExtraNoncontSigs "error" 556 | StateNameClashWarn "none" 557 | SimStateInterfaceChecksumMismatchMsg "warning" 558 | SimStateOlderReleaseMsg "error" 559 | InitInArrayFormatMsg "warning" 560 | StrictBusMsg "ErrorLevel1" 561 | BusNameAdapt "WarnAndRepair" 562 | NonBusSignalsTreatedAsBus "none" 563 | SymbolicDimMinMaxWarning "warning" 564 | LossOfSymbolicDimsSimulationWarning "warning" 565 | LossOfSymbolicDimsCodeGenerationWarning "error" 566 | SymbolicDimsDataTypeCodeGenerationDiagnostic "error" 567 | BlockIODiagnostic "none" 568 | SFUnusedDataAndEventsDiag "warning" 569 | SFUnexpectedBacktrackingDiag "error" 570 | SFInvalidInputDataAccessInChartInitDiag "warning" 571 | SFNoUnconditionalDefaultTransitionDiag "error" 572 | SFTransitionOutsideNaturalParentDiag "warning" 573 | SFUnreachableExecutionPathDiag "warning" 574 | SFUndirectedBroadcastEventsDiag "warning" 575 | SFTransitionActionBeforeConditionDiag "warning" 576 | SFOutputUsedAsStateInMooreChartDiag "error" 577 | SFTemporalDelaySmallerThanSampleTimeDiag "warning" 578 | SFSelfTransitionDiag "warning" 579 | SFExecutionAtInitializationDiag "warning" 580 | SFMachineParentedDataDiag "warning" 581 | IntegerSaturationMsg "warning" 582 | AllowedUnitSystems "all" 583 | UnitsInconsistencyMsg "warning" 584 | AllowAutomaticUnitConversions on 585 | UnitDatabase "" 586 | } 587 | Simulink.HardwareCC { 588 | $ObjectID 15 589 | Version "1.17.0" 590 | DisabledProps [] 591 | Description "" 592 | ProdBitPerChar 8 593 | ProdBitPerShort 16 594 | ProdBitPerInt 32 595 | ProdBitPerLong 32 596 | ProdBitPerLongLong 64 597 | ProdBitPerFloat 32 598 | ProdBitPerDouble 64 599 | ProdBitPerPointer 64 600 | ProdBitPerSizeT 64 601 | ProdBitPerPtrDiffT 64 602 | ProdLargestAtomicInteger "Char" 603 | ProdLargestAtomicFloat "Float" 604 | ProdIntDivRoundTo "Zero" 605 | ProdEndianess "LittleEndian" 606 | ProdWordSize 64 607 | ProdShiftRightIntArith on 608 | ProdLongLongMode off 609 | ProdHWDeviceType "Intel->x86-64 (Windows64)" 610 | TargetBitPerChar 8 611 | TargetBitPerShort 16 612 | TargetBitPerInt 32 613 | TargetBitPerLong 32 614 | TargetBitPerLongLong 64 615 | TargetBitPerFloat 32 616 | TargetBitPerDouble 64 617 | TargetBitPerPointer 32 618 | TargetBitPerSizeT 32 619 | TargetBitPerPtrDiffT 32 620 | TargetLargestAtomicInteger "Char" 621 | TargetLargestAtomicFloat "None" 622 | TargetShiftRightIntArith on 623 | TargetLongLongMode off 624 | TargetIntDivRoundTo "Undefined" 625 | TargetEndianess "Unspecified" 626 | TargetWordSize 32 627 | TargetPreprocMaxBitsSint 32 628 | TargetPreprocMaxBitsUint 32 629 | TargetHWDeviceType "Specified" 630 | TargetUnknown off 631 | ProdEqTarget on 632 | UseEmbeddedCoderFeatures on 633 | UseSimulinkCoderFeatures on 634 | } 635 | Simulink.ModelReferenceCC { 636 | $ObjectID 16 637 | Version "1.17.0" 638 | DisabledProps [] 639 | Description "" 640 | UpdateModelReferenceTargets "IfOutOfDateOrStructuralChange" 641 | EnableRefExpFcnMdlSchedulingChecks on 642 | CheckModelReferenceTargetMessage "error" 643 | EnableParallelModelReferenceBuilds off 644 | ParallelModelReferenceErrorOnInvalidPool on 645 | ParallelModelReferenceMATLABWorkerInit "None" 646 | ModelReferenceNumInstancesAllowed "Multi" 647 | PropagateVarSize "Infer from blocks in model" 648 | ModelDependencies "" 649 | ModelReferencePassRootInputsByReference on 650 | ModelReferenceMinAlgLoopOccurrences off 651 | PropagateSignalLabelsOutOfModel on 652 | SupportModelReferenceSimTargetCustomCode off 653 | } 654 | Simulink.SFSimCC { 655 | $ObjectID 17 656 | Version "1.17.0" 657 | DisabledProps [] 658 | Description "" 659 | SimCustomSourceCode "" 660 | SimCustomHeaderCode "" 661 | SimCustomInitializer "" 662 | SimCustomTerminator "" 663 | SimReservedNameArray [] 664 | SimUserSources "" 665 | SimUserIncludeDirs "" 666 | SimUserLibraries "" 667 | SimUserDefines "" 668 | SimCustomCompilerFlags "" 669 | SimCustomLinkerFlags "" 670 | SFSimEcho on 671 | SimCtrlC on 672 | SimIntegrity on 673 | SimUseLocalCustomCode off 674 | SimParseCustomCode on 675 | SimBuildMode "sf_incremental_build" 676 | SimGenImportedTypeDefs off 677 | ModelFunctionsGlobalVisibility "on" 678 | CompileTimeRecursionLimit 50 679 | EnableRuntimeRecursion on 680 | MATLABDynamicMemAlloc on 681 | MATLABDynamicMemAllocThreshold 65536 682 | } 683 | Simulink.RTWCC { 684 | $BackupClass "Simulink.RTWCC" 685 | $ObjectID 18 686 | Version "1.17.0" 687 | Array { 688 | Type "Cell" 689 | Dimension 13 690 | Cell "IncludeHyperlinkInReport" 691 | Cell "GenerateTraceInfo" 692 | Cell "GenerateTraceReport" 693 | Cell "GenerateTraceReportSl" 694 | Cell "GenerateTraceReportSf" 695 | Cell "GenerateTraceReportEml" 696 | Cell "PortableWordSizes" 697 | Cell "GenerateWebview" 698 | Cell "GenerateCodeMetricsReport" 699 | Cell "GenerateCodeReplacementReport" 700 | Cell "GenerateMissedCodeReplacementReport" 701 | Cell "GenerateErtSFunction" 702 | Cell "CreateSILPILBlock" 703 | PropName "DisabledProps" 704 | } 705 | SystemTargetFile "grt.tlc" 706 | HardwareBoard "None" 707 | TLCOptions "" 708 | GenCodeOnly off 709 | MakeCommand "make_rtw" 710 | GenerateMakefile on 711 | PackageGeneratedCodeAndArtifacts off 712 | PackageName "" 713 | TemplateMakefile "grt_default_tmf" 714 | PostCodeGenCommand "" 715 | Description "" 716 | GenerateReport off 717 | SaveLog off 718 | RTWVerbose on 719 | RetainRTWFile off 720 | RTWBuildHooks [] 721 | ProfileTLC off 722 | TLCDebug off 723 | TLCCoverage off 724 | TLCAssert off 725 | RTWUseLocalCustomCode off 726 | RTWUseSimCustomCode off 727 | CustomSourceCode "" 728 | CustomHeaderCode "" 729 | CustomInclude "" 730 | CustomSource "" 731 | CustomLibrary "" 732 | CustomDefine "" 733 | CustomLAPACKCallback "" 734 | CustomInitializer "" 735 | CustomTerminator "" 736 | Toolchain "Automatically locate an installed toolchain" 737 | BuildConfiguration "Faster Builds" 738 | CustomToolchainOptions [] 739 | IncludeHyperlinkInReport off 740 | LaunchReport off 741 | PortableWordSizes off 742 | CreateSILPILBlock "None" 743 | CodeExecutionProfiling off 744 | CodeExecutionProfileVariable "executionProfile" 745 | CodeProfilingSaveOptions "SummaryOnly" 746 | CodeProfilingInstrumentation off 747 | SILDebugging off 748 | TargetLang "C" 749 | IncludeBusHierarchyInRTWFileBlockHierarchyMap off 750 | GenerateTraceInfo off 751 | GenerateTraceReport off 752 | GenerateTraceReportSl off 753 | GenerateTraceReportSf off 754 | GenerateTraceReportEml off 755 | GenerateWebview off 756 | GenerateCodeMetricsReport off 757 | GenerateCodeReplacementReport off 758 | GenerateMissedCodeReplacementReport off 759 | RTWCompilerOptimization "off" 760 | ObjectivePriorities [] 761 | RTWCustomCompilerOptimizations "" 762 | CheckMdlBeforeBuild "Off" 763 | SharedConstantsCachingThreshold 1024 764 | Array { 765 | Type "Handle" 766 | Dimension 2 767 | Simulink.CodeAppCC { 768 | $ObjectID 19 769 | Version "1.17.0" 770 | Array { 771 | Type "Cell" 772 | Dimension 27 773 | Cell "IgnoreCustomStorageClasses" 774 | Cell "IgnoreTestpoints" 775 | Cell "InsertBlockDesc" 776 | Cell "InsertPolySpaceComments" 777 | Cell "SFDataObjDesc" 778 | Cell "MATLABFcnDesc" 779 | Cell "SimulinkDataObjDesc" 780 | Cell "DefineNamingRule" 781 | Cell "SignalNamingRule" 782 | Cell "ParamNamingRule" 783 | Cell "InternalIdentifier" 784 | Cell "InlinedPrmAccess" 785 | Cell "CustomSymbolStr" 786 | Cell "CustomSymbolStrGlobalVar" 787 | Cell "CustomSymbolStrType" 788 | Cell "CustomSymbolStrField" 789 | Cell "CustomSymbolStrFcn" 790 | Cell "CustomSymbolStrModelFcn" 791 | Cell "CustomSymbolStrFcnArg" 792 | Cell "CustomSymbolStrBlkIO" 793 | Cell "CustomSymbolStrTmpVar" 794 | Cell "CustomSymbolStrMacro" 795 | Cell "CustomSymbolStrUtil" 796 | Cell "CustomUserTokenString" 797 | Cell "ReqsInCode" 798 | Cell "CustomSymbolStrEmxType" 799 | Cell "CustomSymbolStrEmxFcn" 800 | PropName "DisabledProps" 801 | } 802 | Description "" 803 | Comment "" 804 | ForceParamTrailComments off 805 | GenerateComments on 806 | CommentStyle "Auto" 807 | IgnoreCustomStorageClasses on 808 | IgnoreTestpoints off 809 | IncHierarchyInIds off 810 | MaxIdLength 31 811 | PreserveName off 812 | PreserveNameWithParent off 813 | ShowEliminatedStatement off 814 | OperatorAnnotations off 815 | IncAutoGenComments off 816 | SimulinkDataObjDesc off 817 | SFDataObjDesc off 818 | MATLABFcnDesc off 819 | IncDataTypeInIds off 820 | MangleLength 1 821 | CustomSymbolStrGlobalVar "$R$N$M" 822 | CustomSymbolStrType "$N$R$M_T" 823 | CustomSymbolStrField "$N$M" 824 | CustomSymbolStrFcn "$R$N$M$F" 825 | CustomSymbolStrModelFcn "$R$N" 826 | CustomSymbolStrFcnArg "rt$I$N$M" 827 | CustomSymbolStrBlkIO "rtb_$N$M" 828 | CustomSymbolStrTmpVar "$N$M" 829 | CustomSymbolStrMacro "$R$N$M" 830 | CustomSymbolStrUtil "$N$C" 831 | CustomSymbolStrEmxType "emxArray_$M$N" 832 | CustomSymbolStrEmxFcn "emx$M$N" 833 | CustomUserTokenString "" 834 | CustomCommentsFcn "" 835 | DefineNamingRule "None" 836 | DefineNamingFcn "" 837 | ParamNamingRule "None" 838 | ParamNamingFcn "" 839 | SignalNamingRule "None" 840 | SignalNamingFcn "" 841 | InsertBlockDesc off 842 | InsertPolySpaceComments off 843 | SimulinkBlockComments on 844 | MATLABSourceComments off 845 | EnableCustomComments off 846 | InternalIdentifierFile "" 847 | InternalIdentifier "Shortened" 848 | InlinedPrmAccess "Literals" 849 | ReqsInCode off 850 | UseSimReservedNames off 851 | ReservedNameArray [] 852 | } 853 | Simulink.GRTTargetCC { 854 | $BackupClass "Simulink.TargetCC" 855 | $ObjectID 20 856 | Version "1.17.0" 857 | Array { 858 | Type "Cell" 859 | Dimension 15 860 | Cell "IncludeMdlTerminateFcn" 861 | Cell "SuppressErrorStatus" 862 | Cell "ERTCustomFileBanners" 863 | Cell "GenerateSampleERTMain" 864 | Cell "ExistingSharedCode" 865 | Cell "GenerateTestInterfaces" 866 | Cell "ModelStepFunctionPrototypeControlCompliant" 867 | Cell "GenerateAllocFcn" 868 | Cell "PurelyIntegerCode" 869 | Cell "SupportComplex" 870 | Cell "SupportAbsoluteTime" 871 | Cell "SupportContinuousTime" 872 | Cell "SupportNonInlinedSFcns" 873 | Cell "RemoveDisableFunc" 874 | Cell "RemoveResetFunc" 875 | PropName "DisabledProps" 876 | } 877 | Description "" 878 | TargetFcnLib "ansi_tfl_table_tmw.mat" 879 | TargetLibSuffix "" 880 | TargetPreCompLibLocation "" 881 | GenFloatMathFcnCalls "NOT IN USE" 882 | TargetLangStandard "C99 (ISO)" 883 | CodeReplacementLibrary "None" 884 | UtilityFuncGeneration "Auto" 885 | MultiwordTypeDef "System defined" 886 | MultiwordLength 2048 887 | GenerateFullHeader on 888 | InferredTypesCompatibility off 889 | ExistingSharedCode "" 890 | GenerateSampleERTMain off 891 | GenerateTestInterfaces off 892 | ModelReferenceCompliant on 893 | ParMdlRefBuildCompliant on 894 | CompOptLevelCompliant on 895 | ConcurrentExecutionCompliant on 896 | IncludeMdlTerminateFcn on 897 | GeneratePreprocessorConditionals "Use local settings" 898 | CombineOutputUpdateFcns on 899 | CombineSignalStateStructs off 900 | SuppressErrorStatus off 901 | ERTFirstTimeCompliant off 902 | IncludeFileDelimiter "Auto" 903 | ERTCustomFileBanners off 904 | SupportAbsoluteTime on 905 | LogVarNameModifier "rt_" 906 | MatFileLogging on 907 | MultiInstanceERTCode off 908 | CodeInterfacePackaging "Nonreusable function" 909 | SupportNonFinite on 910 | SupportComplex on 911 | PurelyIntegerCode off 912 | SupportContinuousTime on 913 | SupportNonInlinedSFcns on 914 | RemoveDisableFunc off 915 | RemoveResetFunc off 916 | SupportVariableSizeSignals off 917 | ParenthesesLevel "Nominal" 918 | CastingMode "Nominal" 919 | MATLABClassNameForMDSCustomization "Simulink.SoftwareTarget.GRTCustomization" 920 | ModelStepFunctionPrototypeControlCompliant off 921 | CPPClassGenCompliant on 922 | AutosarCompliant off 923 | MDXCompliant off 924 | GRTInterface off 925 | GenerateAllocFcn off 926 | UseToolchainInfoCompliant on 927 | GenerateSharedConstants on 928 | CoderGroups [] 929 | AccessMethods [] 930 | LookupTableObjectStructAxisOrder "1,2,3,4,..." 931 | UseMalloc off 932 | ExtMode off 933 | ExtModeStaticAlloc off 934 | ExtModeTesting off 935 | ExtModeStaticAllocSize 1000000 936 | ExtModeTransport 0 937 | ExtModeMexFile "ext_comm" 938 | ExtModeMexArgs "" 939 | ExtModeIntrfLevel "Level1" 940 | RTWCAPISignals off 941 | RTWCAPIParams off 942 | RTWCAPIStates off 943 | RTWCAPIRootIO off 944 | GenerateASAP2 off 945 | MultiInstanceErrorCode "Error" 946 | } 947 | PropName "Components" 948 | } 949 | } 950 | SlCovCC.ConfigComp { 951 | $ObjectID 21 952 | Version "1.17.0" 953 | DisabledProps [] 954 | Description "Simulink Coverage Configuration Component" 955 | Name "Simulink Coverage" 956 | CovEnable off 957 | CovScope "EntireSystem" 958 | CovIncludeTopModel on 959 | RecordCoverage off 960 | CovPath "/" 961 | CovSaveName "covdata" 962 | CovCompData "" 963 | CovMetricSettings "dwe" 964 | CovFilter "" 965 | CovHTMLOptions "" 966 | CovNameIncrementing off 967 | CovHtmlReporting off 968 | CovForceBlockReductionOff on 969 | CovEnableCumulative on 970 | CovSaveCumulativeToWorkspaceVar off 971 | CovSaveSingleToWorkspaceVar off 972 | CovCumulativeVarName "covCumulativeData" 973 | CovCumulativeReport off 974 | CovSaveOutputData on 975 | CovOutputDir "slcov_output/$ModelName$" 976 | CovDataFileName "$ModelName$_cvdata" 977 | CovShowResultsExplorer on 978 | CovReportOnPause on 979 | CovModelRefEnable "off" 980 | CovModelRefExcluded "" 981 | CovExternalEMLEnable on 982 | CovSFcnEnable on 983 | CovBoundaryAbsTol 1e-05 984 | CovBoundaryRelTol 0.01 985 | CovUseTimeInterval off 986 | CovStartTime 0 987 | CovStopTime 0 988 | CovMcdcMode "Masking" 989 | } 990 | hdlcoderui.hdlcc { 991 | $ObjectID 22 992 | Version "1.17.0" 993 | DisabledProps [] 994 | Description "HDL Coder custom configuration component" 995 | Name "HDL Coder" 996 | Array { 997 | Type "Cell" 998 | Dimension 1 999 | Cell " " 1000 | PropName "HDLConfigFile" 1001 | } 1002 | HDLCActiveTab "0" 1003 | } 1004 | PropName "Components" 1005 | } 1006 | Name "Configuration" 1007 | ExtraOptions "" 1008 | CurrentDlgPage "Solver" 1009 | ConfigPrmDlgPosition [ 282, 114, 1256, 728 ] 1010 | } 1011 | PropName "ConfigurationSets" 1012 | } 1013 | Simulink.ConfigSet { 1014 | $PropName "ActiveConfigurationSet" 1015 | $ObjectID 10 1016 | } 1017 | Object { 1018 | $PropName "DataTransfer" 1019 | $ObjectID 23 1020 | $ClassName "Simulink.GlobalDataTransfer" 1021 | DefaultTransitionBetweenSyncTasks "Ensure deterministic transfer (maximum delay)" 1022 | DefaultTransitionBetweenAsyncTasks "Ensure data integrity only" 1023 | DefaultTransitionBetweenContTasks "Ensure deterministic transfer (minimum delay)" 1024 | DefaultExtrapolationMethodBetweenContTasks "None" 1025 | AutoInsertRateTranBlk [0] 1026 | } 1027 | ExplicitPartitioning off 1028 | BlockDefaults { 1029 | ForegroundColor "black" 1030 | BackgroundColor "white" 1031 | DropShadow off 1032 | NamePlacement "normal" 1033 | FontName "Helvetica" 1034 | FontSize 10 1035 | FontWeight "normal" 1036 | FontAngle "normal" 1037 | ShowName on 1038 | HideAutomaticName on 1039 | BlockRotation 0 1040 | BlockMirror off 1041 | } 1042 | AnnotationDefaults { 1043 | HorizontalAlignment "center" 1044 | VerticalAlignment "middle" 1045 | ForegroundColor "black" 1046 | BackgroundColor "white" 1047 | DropShadow off 1048 | FontName "Helvetica" 1049 | FontSize 10 1050 | FontWeight "normal" 1051 | FontAngle "normal" 1052 | UseDisplayTextAsClickCallback off 1053 | } 1054 | LineDefaults { 1055 | FontName "Helvetica" 1056 | FontSize 9 1057 | FontWeight "normal" 1058 | FontAngle "normal" 1059 | } 1060 | MaskDefaults { 1061 | SelfModifiable "off" 1062 | IconFrame "on" 1063 | IconOpaque "opaque" 1064 | RunInitForIconRedraw "analyze" 1065 | IconRotate "none" 1066 | PortRotate "default" 1067 | IconUnits "autoscale" 1068 | } 1069 | MaskParameterDefaults { 1070 | Evaluate "on" 1071 | Tunable "on" 1072 | NeverSave "off" 1073 | Internal "off" 1074 | ReadOnly "off" 1075 | Enabled "on" 1076 | Visible "on" 1077 | ToolTip "on" 1078 | } 1079 | BlockParameterDefaults { 1080 | Block { 1081 | BlockType Demux 1082 | Outputs "4" 1083 | DisplayOption "none" 1084 | BusSelectionMode off 1085 | } 1086 | Block { 1087 | BlockType From 1088 | GotoTag "A" 1089 | IconDisplay "Tag" 1090 | TagVisibility "local" 1091 | } 1092 | Block { 1093 | BlockType Goto 1094 | GotoTag "A" 1095 | IconDisplay "Tag" 1096 | TagVisibility "local" 1097 | } 1098 | Block { 1099 | BlockType If 1100 | NumInputs "1" 1101 | IfExpression "u1 > 0" 1102 | ShowElse on 1103 | ZeroCross on 1104 | SampleTime "-1" 1105 | } 1106 | Block { 1107 | BlockType Logic 1108 | Operator "AND" 1109 | Inputs "2" 1110 | IconShape "rectangular" 1111 | AllPortsSameDT on 1112 | OutDataTypeStr "Inherit: Logical (see Configuration Parameters: Optimization)" 1113 | SampleTime "-1" 1114 | } 1115 | Block { 1116 | BlockType Outport 1117 | Port "1" 1118 | OutMin "[]" 1119 | OutMax "[]" 1120 | OutDataTypeStr "Inherit: auto" 1121 | LockScale off 1122 | BusOutputAsStruct off 1123 | Unit "inherit" 1124 | PortDimensions "-1" 1125 | VarSizeSig "Inherit" 1126 | SampleTime "-1" 1127 | SignalType "auto" 1128 | SamplingMode "auto" 1129 | EnsureOutportIsVirtual off 1130 | SourceOfInitialOutputValue "Dialog" 1131 | OutputWhenDisabled "held" 1132 | InitialOutput "[]" 1133 | MustResolveToSignalObject off 1134 | } 1135 | Block { 1136 | BlockType SubSystem 1137 | ShowPortLabels "FromPortIcon" 1138 | Permissions "ReadWrite" 1139 | PermitHierarchicalResolution "All" 1140 | TreatAsAtomicUnit off 1141 | MinAlgLoopOccurrences off 1142 | PropExecContextOutsideSubsystem off 1143 | SystemSampleTime "-1" 1144 | RTWSystemCode "Auto" 1145 | RTWFcnNameOpts "Auto" 1146 | RTWFileNameOpts "Auto" 1147 | FunctionInterfaceSpec "void_void" 1148 | FunctionWithSeparateData off 1149 | RTWMemSecFuncInitTerm "Inherit from model" 1150 | RTWMemSecFuncExecute "Inherit from model" 1151 | RTWMemSecDataConstants "Inherit from model" 1152 | RTWMemSecDataInternal "Inherit from model" 1153 | RTWMemSecDataParameters "Inherit from model" 1154 | SimViewingDevice off 1155 | DataTypeOverride "UseLocalSettings" 1156 | DataTypeOverrideAppliesTo "AllNumericTypes" 1157 | MinMaxOverflowLogging "UseLocalSettings" 1158 | Opaque off 1159 | MaskHideContents off 1160 | SFBlockType "NONE" 1161 | GeneratePreprocessorConditionals off 1162 | PropagateVariantConditions off 1163 | TreatAsGroupedWhenPropagatingVariantConditions on 1164 | ContentPreviewEnabled off 1165 | IsWebBlock off 1166 | } 1167 | Block { 1168 | BlockType ToWorkspace 1169 | VariableName "simulink_output" 1170 | MaxDataPoints "1000" 1171 | Decimation "1" 1172 | SaveFormat "Array" 1173 | Save2DSignal "Inherit from input (this choice will be removed - see release notes)" 1174 | FixptAsFi off 1175 | NumInputs "1" 1176 | SampleTime "0" 1177 | } 1178 | } 1179 | System { 1180 | Name "trajTest" 1181 | Location [8, 8, 1558, 846] 1182 | Open on 1183 | PortBlocksUseCompactNotation off 1184 | ModelBrowserVisibility off 1185 | ModelBrowserWidth 200 1186 | ScreenColor "white" 1187 | PaperOrientation "landscape" 1188 | PaperPositionMode "auto" 1189 | PaperType "usletter" 1190 | PaperUnits "inches" 1191 | TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] 1192 | TiledPageScale 1 1193 | ShowPageBoundaries off 1194 | ZoomFactor "100" 1195 | ReportName "simulink-default.rpt" 1196 | SIDHighWatermark "32" 1197 | Block { 1198 | BlockType Demux 1199 | Name "Demux1" 1200 | SID "1" 1201 | Ports [1, 5] 1202 | Position [365, 63, 370, 237] 1203 | ZOrder 126 1204 | ShowName off 1205 | Outputs "5" 1206 | DisplayOption "bar" 1207 | Port { 1208 | PortNumber 1 1209 | Name "snap" 1210 | } 1211 | Port { 1212 | PortNumber 2 1213 | Name "jerk" 1214 | } 1215 | Port { 1216 | PortNumber 3 1217 | Name "acc" 1218 | } 1219 | Port { 1220 | PortNumber 4 1221 | Name "vel" 1222 | } 1223 | Port { 1224 | PortNumber 5 1225 | Name "dis" 1226 | } 1227 | } 1228 | Block { 1229 | BlockType From 1230 | Name "From" 1231 | SID "16" 1232 | Position [575, 105, 615, 135] 1233 | ZOrder 141 1234 | GotoTag "jerk" 1235 | } 1236 | Block { 1237 | BlockType From 1238 | Name "From1" 1239 | SID "17" 1240 | Position [575, 160, 615, 190] 1241 | ZOrder 142 1242 | GotoTag "acc" 1243 | } 1244 | Block { 1245 | BlockType From 1246 | Name "From2" 1247 | SID "23" 1248 | Position [575, 215, 615, 245] 1249 | ZOrder 148 1250 | GotoTag "vel" 1251 | } 1252 | Block { 1253 | BlockType From 1254 | Name "From3" 1255 | SID "24" 1256 | Position [575, 280, 615, 310] 1257 | ZOrder 149 1258 | GotoTag "dis" 1259 | } 1260 | Block { 1261 | BlockType From 1262 | Name "From4" 1263 | SID "25" 1264 | Position [575, 55, 615, 85] 1265 | ZOrder 150 1266 | GotoTag "snap" 1267 | } 1268 | Block { 1269 | BlockType From 1270 | Name "From5" 1271 | SID "27" 1272 | Position [575, 340, 615, 370] 1273 | ZOrder 152 1274 | GotoTag "dis" 1275 | } 1276 | Block { 1277 | BlockType Goto 1278 | Name "Goto2" 1279 | SID "2" 1280 | Position [430, 65, 470, 95] 1281 | ZOrder 127 1282 | GotoTag "snap" 1283 | } 1284 | Block { 1285 | BlockType Goto 1286 | Name "Goto3" 1287 | SID "3" 1288 | Position [430, 100, 470, 130] 1289 | ZOrder 128 1290 | GotoTag "jerk" 1291 | } 1292 | Block { 1293 | BlockType Goto 1294 | Name "Goto4" 1295 | SID "4" 1296 | Position [430, 135, 470, 165] 1297 | ZOrder 129 1298 | GotoTag "acc" 1299 | } 1300 | Block { 1301 | BlockType Goto 1302 | Name "Goto5" 1303 | SID "5" 1304 | Position [430, 170, 470, 200] 1305 | ZOrder 130 1306 | GotoTag "vel" 1307 | } 1308 | Block { 1309 | BlockType Goto 1310 | Name "Goto6" 1311 | SID "6" 1312 | Position [430, 205, 470, 235] 1313 | ZOrder 132 1314 | GotoTag "dis" 1315 | } 1316 | Block { 1317 | BlockType If 1318 | Name "If" 1319 | SID "32" 1320 | Ports [1, 2] 1321 | Position [735, 461, 835, 499] 1322 | ZOrder 157 1323 | } 1324 | Block { 1325 | BlockType Logic 1326 | Name "Logical\nOperator" 1327 | SID "31" 1328 | Ports [2, 1] 1329 | Position [545, 407, 575, 438] 1330 | ZOrder 156 1331 | AllPortsSameDT off 1332 | OutDataTypeStr "boolean" 1333 | } 1334 | Block { 1335 | BlockType ToWorkspace 1336 | Name "To Workspace" 1337 | SID "18" 1338 | Ports [1] 1339 | Position [655, 105, 715, 135] 1340 | ZOrder 143 1341 | VariableName "jerk" 1342 | MaxDataPoints "inf" 1343 | SaveFormat "Structure With Time" 1344 | Save2DSignal "3-D array (concatenate along third dimension)" 1345 | FixptAsFi on 1346 | SampleTime "-1" 1347 | } 1348 | Block { 1349 | BlockType ToWorkspace 1350 | Name "To Workspace1" 1351 | SID "19" 1352 | Ports [1] 1353 | Position [655, 160, 715, 190] 1354 | ZOrder 144 1355 | VariableName "acc" 1356 | MaxDataPoints "inf" 1357 | SaveFormat "Structure With Time" 1358 | Save2DSignal "3-D array (concatenate along third dimension)" 1359 | FixptAsFi on 1360 | SampleTime "-1" 1361 | } 1362 | Block { 1363 | BlockType ToWorkspace 1364 | Name "To Workspace2" 1365 | SID "21" 1366 | Ports [1] 1367 | Position [655, 215, 715, 245] 1368 | ZOrder 146 1369 | VariableName "vel" 1370 | MaxDataPoints "inf" 1371 | SaveFormat "Structure With Time" 1372 | Save2DSignal "3-D array (concatenate along third dimension)" 1373 | FixptAsFi on 1374 | SampleTime "-1" 1375 | } 1376 | Block { 1377 | BlockType ToWorkspace 1378 | Name "To Workspace3" 1379 | SID "22" 1380 | Ports [1] 1381 | Position [655, 280, 715, 310] 1382 | ZOrder 147 1383 | VariableName "dis" 1384 | MaxDataPoints "inf" 1385 | SaveFormat "Structure With Time" 1386 | Save2DSignal "3-D array (concatenate along third dimension)" 1387 | FixptAsFi on 1388 | SampleTime "-1" 1389 | } 1390 | Block { 1391 | BlockType ToWorkspace 1392 | Name "To Workspace4" 1393 | SID "26" 1394 | Ports [1] 1395 | Position [655, 55, 715, 85] 1396 | ZOrder 151 1397 | VariableName "snap" 1398 | MaxDataPoints "inf" 1399 | SaveFormat "Structure With Time" 1400 | Save2DSignal "3-D array (concatenate along third dimension)" 1401 | FixptAsFi on 1402 | SampleTime "-1" 1403 | } 1404 | Block { 1405 | BlockType ToWorkspace 1406 | Name "To Workspace5" 1407 | SID "28" 1408 | Ports [1] 1409 | Position [655, 340, 715, 370] 1410 | ZOrder 153 1411 | VariableName "traj" 1412 | MaxDataPoints "inf" 1413 | FixptAsFi on 1414 | SampleTime "0.0002" 1415 | } 1416 | Block { 1417 | BlockType SubSystem 1418 | Name "traj" 1419 | SID "7" 1420 | Ports [0, 1] 1421 | Position [30, 179, 130, 221] 1422 | ZOrder 131 1423 | Commented "on" 1424 | AttributesFormatString "%" 1425 | LinkData { 1426 | BlockName "4th order traj" 1427 | DialogParameters { 1428 | v "0.5" 1429 | x "0.1" 1430 | j "500" 1431 | } 1432 | } 1433 | BlockChoice "4th order traj" 1434 | TemplateBlock "model_controller/Profile Generators/traj" 1435 | MemberBlocks "2th order traj,3rd order traj,4th order traj,step" 1436 | RequestExecContextInheritance off 1437 | Variant off 1438 | System { 1439 | Name "traj" 1440 | Location [148, 182, 646, 482] 1441 | Open off 1442 | PortBlocksUseCompactNotation off 1443 | ModelBrowserVisibility off 1444 | ModelBrowserWidth 200 1445 | ScreenColor "white" 1446 | PaperOrientation "landscape" 1447 | PaperPositionMode "auto" 1448 | PaperType "usletter" 1449 | PaperUnits "inches" 1450 | TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] 1451 | TiledPageScale 1 1452 | ShowPageBoundaries off 1453 | ZoomFactor "100" 1454 | SIDHighWatermark "262" 1455 | Block { 1456 | BlockType Reference 1457 | Name "4th order traj" 1458 | SID "7::214" 1459 | Ports [0, 1] 1460 | Position [100, 40, 140, 80] 1461 | ZOrder 77 1462 | LibraryVersion "1.33" 1463 | SourceBlock "model_controller/Profile Generators/4th order traj" 1464 | SourceType "4th Order Profile Generator" 1465 | ContentPreviewEnabled off 1466 | t0 "0" 1467 | x "0.1" 1468 | v "0.5" 1469 | a "10" 1470 | j "500" 1471 | d "64000" 1472 | Ts " 0" 1473 | } 1474 | Block { 1475 | BlockType Outport 1476 | Name "Out1" 1477 | SID "7::158" 1478 | Position [200, 40, 220, 60] 1479 | ZOrder -1 1480 | IconDisplay "Port number" 1481 | } 1482 | Line { 1483 | ZOrder 3 1484 | SrcBlock "4th order traj" 1485 | SrcPort 1 1486 | DstBlock "Out1" 1487 | DstPort 1 1488 | } 1489 | } 1490 | } 1491 | Block { 1492 | BlockType SubSystem 1493 | Name "traj1" 1494 | SID "14" 1495 | Ports [0, 1] 1496 | Position [30, 19, 130, 61] 1497 | ZOrder 138 1498 | Commented "on" 1499 | AttributesFormatString "%" 1500 | LinkData { 1501 | BlockName "4th order traj" 1502 | DialogParameters { 1503 | v "0.25" 1504 | } 1505 | } 1506 | BlockChoice "4th order traj" 1507 | TemplateBlock "model_controller/Profile Generators/traj" 1508 | MemberBlocks "2th order traj,3rd order traj,4th order traj,step" 1509 | RequestExecContextInheritance off 1510 | Variant off 1511 | System { 1512 | Name "traj1" 1513 | Location [148, 182, 646, 482] 1514 | Open off 1515 | PortBlocksUseCompactNotation off 1516 | ModelBrowserVisibility off 1517 | ModelBrowserWidth 200 1518 | ScreenColor "white" 1519 | PaperOrientation "landscape" 1520 | PaperPositionMode "auto" 1521 | PaperType "usletter" 1522 | PaperUnits "inches" 1523 | TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] 1524 | TiledPageScale 1 1525 | ShowPageBoundaries off 1526 | ZoomFactor "100" 1527 | SIDHighWatermark "262" 1528 | Block { 1529 | BlockType Reference 1530 | Name "4th order traj" 1531 | SID "14::214" 1532 | Ports [0, 1] 1533 | Position [100, 40, 140, 80] 1534 | ZOrder 77 1535 | LibraryVersion "1.33" 1536 | SourceBlock "model_controller/Profile Generators/4th order traj" 1537 | SourceType "4th Order Profile Generator" 1538 | ContentPreviewEnabled off 1539 | t0 "0" 1540 | x "0.06" 1541 | v "0.25" 1542 | a "10" 1543 | j "800" 1544 | d "64000" 1545 | Ts " 0" 1546 | } 1547 | Block { 1548 | BlockType Outport 1549 | Name "Out1" 1550 | SID "14::158" 1551 | Position [200, 40, 220, 60] 1552 | ZOrder -1 1553 | IconDisplay "Port number" 1554 | } 1555 | Line { 1556 | ZOrder 3 1557 | SrcBlock "4th order traj" 1558 | SrcPort 1 1559 | DstBlock "Out1" 1560 | DstPort 1 1561 | } 1562 | } 1563 | } 1564 | Block { 1565 | BlockType SubSystem 1566 | Name "traj2" 1567 | SID "15" 1568 | Ports [0, 1] 1569 | Position [30, 109, 130, 151] 1570 | ZOrder 140 1571 | Commented "on" 1572 | AttributesFormatString "%" 1573 | LinkData { 1574 | BlockName "4th order traj" 1575 | DialogParameters { 1576 | v "0.6" 1577 | x "0.1" 1578 | j "700" 1579 | d "20000" 1580 | } 1581 | } 1582 | BlockChoice "4th order traj" 1583 | TemplateBlock "model_controller/Profile Generators/traj" 1584 | MemberBlocks "2th order traj,3rd order traj,4th order traj,step" 1585 | RequestExecContextInheritance off 1586 | Variant off 1587 | System { 1588 | Name "traj2" 1589 | Location [148, 182, 646, 482] 1590 | Open off 1591 | PortBlocksUseCompactNotation off 1592 | ModelBrowserVisibility off 1593 | ModelBrowserWidth 200 1594 | ScreenColor "white" 1595 | PaperOrientation "landscape" 1596 | PaperPositionMode "auto" 1597 | PaperType "usletter" 1598 | PaperUnits "inches" 1599 | TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] 1600 | TiledPageScale 1 1601 | ShowPageBoundaries off 1602 | ZoomFactor "100" 1603 | SIDHighWatermark "262" 1604 | Block { 1605 | BlockType Reference 1606 | Name "4th order traj" 1607 | SID "15::214" 1608 | Ports [0, 1] 1609 | Position [100, 40, 140, 80] 1610 | ZOrder 77 1611 | LibraryVersion "1.33" 1612 | SourceBlock "model_controller/Profile Generators/4th order traj" 1613 | SourceType "4th Order Profile Generator" 1614 | ContentPreviewEnabled off 1615 | t0 "0" 1616 | x "0.1" 1617 | v "0.6" 1618 | a "10" 1619 | j "700" 1620 | d "20000" 1621 | Ts " 0" 1622 | } 1623 | Block { 1624 | BlockType Outport 1625 | Name "Out1" 1626 | SID "15::158" 1627 | Position [200, 40, 220, 60] 1628 | ZOrder -1 1629 | IconDisplay "Port number" 1630 | } 1631 | Line { 1632 | ZOrder 3 1633 | SrcBlock "4th order traj" 1634 | SrcPort 1 1635 | DstBlock "Out1" 1636 | DstPort 1 1637 | } 1638 | } 1639 | } 1640 | Block { 1641 | BlockType SubSystem 1642 | Name "traj3" 1643 | SID "20" 1644 | Ports [0, 1] 1645 | Position [170, 54, 270, 96] 1646 | ZOrder 145 1647 | AttributesFormatString "%" 1648 | LinkData { 1649 | BlockName "3rd order traj" 1650 | DialogParameters { 1651 | x "0.06" 1652 | v "0.25" 1653 | a "10" 1654 | j "800" 1655 | t0 "0" 1656 | } 1657 | BlockName "4th order traj" 1658 | DialogParameters { 1659 | v "0.25" 1660 | x "0.05" 1661 | a "10" 1662 | d "64000" 1663 | j "700" 1664 | } 1665 | } 1666 | BlockChoice "4th order traj" 1667 | TemplateBlock "model_controller/Profile Generators/traj" 1668 | MemberBlocks "2th order traj,3rd order traj,4th order traj,step" 1669 | RequestExecContextInheritance off 1670 | Variant off 1671 | System { 1672 | Name "traj3" 1673 | Location [148, 182, 646, 482] 1674 | Open off 1675 | PortBlocksUseCompactNotation off 1676 | ModelBrowserVisibility off 1677 | ModelBrowserWidth 200 1678 | ScreenColor "white" 1679 | PaperOrientation "landscape" 1680 | PaperPositionMode "auto" 1681 | PaperType "usletter" 1682 | PaperUnits "inches" 1683 | TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] 1684 | TiledPageScale 1 1685 | ShowPageBoundaries off 1686 | ZoomFactor "100" 1687 | SIDHighWatermark "262" 1688 | Block { 1689 | BlockType Reference 1690 | Name "4th order traj" 1691 | SID "20::214" 1692 | Ports [0, 1] 1693 | Position [100, 40, 140, 80] 1694 | ZOrder 77 1695 | LibraryVersion "1.33" 1696 | SourceBlock "model_controller/Profile Generators/4th order traj" 1697 | SourceType "4th Order Profile Generator" 1698 | ContentPreviewEnabled off 1699 | t0 "0" 1700 | x "0.05" 1701 | v "0.25" 1702 | a "10" 1703 | j "700" 1704 | d "64000" 1705 | Ts " 0" 1706 | } 1707 | Block { 1708 | BlockType Outport 1709 | Name "Out1" 1710 | SID "20::158" 1711 | Position [200, 40, 220, 60] 1712 | ZOrder -1 1713 | IconDisplay "Port number" 1714 | } 1715 | Line { 1716 | ZOrder 3 1717 | SrcBlock "4th order traj" 1718 | SrcPort 1 1719 | DstBlock "Out1" 1720 | DstPort 1 1721 | } 1722 | } 1723 | } 1724 | Block { 1725 | BlockType SubSystem 1726 | Name "traj4" 1727 | SID "29" 1728 | Ports [0, 1] 1729 | Position [170, 129, 270, 171] 1730 | ZOrder 154 1731 | AttributesFormatString "%" 1732 | LinkData { 1733 | BlockName "3rd order traj" 1734 | DialogParameters { 1735 | x "0.0012" 1736 | v "1" 1737 | a "30" 1738 | j "2000" 1739 | t0 "0" 1740 | } 1741 | BlockName "4th order traj" 1742 | DialogParameters { 1743 | v "0.5" 1744 | x "0.05" 1745 | a "20" 1746 | d "1600000" 1747 | j "2000" 1748 | } 1749 | } 1750 | BlockChoice "3rd order traj" 1751 | TemplateBlock "model_controller/Profile Generators/traj" 1752 | MemberBlocks "2th order traj,3rd order traj,4th order traj,step" 1753 | RequestExecContextInheritance off 1754 | Variant off 1755 | System { 1756 | Name "traj4" 1757 | Location [148, 182, 646, 482] 1758 | Open off 1759 | PortBlocksUseCompactNotation off 1760 | ModelBrowserVisibility off 1761 | ModelBrowserWidth 200 1762 | ScreenColor "white" 1763 | PaperOrientation "landscape" 1764 | PaperPositionMode "auto" 1765 | PaperType "usletter" 1766 | PaperUnits "inches" 1767 | TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] 1768 | TiledPageScale 1 1769 | ShowPageBoundaries off 1770 | ZoomFactor "100" 1771 | SIDHighWatermark "262" 1772 | Block { 1773 | BlockType Reference 1774 | Name "3rd order traj" 1775 | SID "29::167" 1776 | Ports [0, 1] 1777 | Position [100, 40, 140, 80] 1778 | ZOrder 74 1779 | LibraryVersion "1.33" 1780 | SourceBlock "model_controller/Profile Generators/3rd order traj" 1781 | SourceType "3d Order Profile Generator" 1782 | ContentPreviewEnabled off 1783 | t0 "0" 1784 | x "0.0012" 1785 | v "1" 1786 | a "30" 1787 | j "2000" 1788 | Ts " 0" 1789 | } 1790 | Block { 1791 | BlockType Outport 1792 | Name "Out1" 1793 | SID "29::158" 1794 | Position [200, 40, 220, 60] 1795 | ZOrder -1 1796 | IconDisplay "Port number" 1797 | } 1798 | Line { 1799 | ZOrder 3 1800 | SrcBlock "3rd order traj" 1801 | SrcPort 1 1802 | DstBlock "Out1" 1803 | DstPort 1 1804 | } 1805 | } 1806 | } 1807 | Line { 1808 | Name "snap" 1809 | ZOrder 1 1810 | Labels [1, 1] 1811 | SrcBlock "Demux1" 1812 | SrcPort 1 1813 | DstBlock "Goto2" 1814 | DstPort 1 1815 | } 1816 | Line { 1817 | Name "jerk" 1818 | ZOrder 2 1819 | Labels [1, 1] 1820 | SrcBlock "Demux1" 1821 | SrcPort 2 1822 | DstBlock "Goto3" 1823 | DstPort 1 1824 | } 1825 | Line { 1826 | Name "acc" 1827 | ZOrder 3 1828 | Labels [1, 1] 1829 | SrcBlock "Demux1" 1830 | SrcPort 3 1831 | DstBlock "Goto4" 1832 | DstPort 1 1833 | } 1834 | Line { 1835 | Name "vel" 1836 | ZOrder 4 1837 | Labels [1, 1] 1838 | SrcBlock "Demux1" 1839 | SrcPort 4 1840 | DstBlock "Goto5" 1841 | DstPort 1 1842 | } 1843 | Line { 1844 | Name "dis" 1845 | ZOrder 5 1846 | Labels [1, 1] 1847 | SrcBlock "Demux1" 1848 | SrcPort 5 1849 | DstBlock "Goto6" 1850 | DstPort 1 1851 | } 1852 | Line { 1853 | ZOrder 8 1854 | SrcBlock "From1" 1855 | SrcPort 1 1856 | DstBlock "To Workspace1" 1857 | DstPort 1 1858 | } 1859 | Line { 1860 | ZOrder 9 1861 | SrcBlock "From" 1862 | SrcPort 1 1863 | DstBlock "To Workspace" 1864 | DstPort 1 1865 | } 1866 | Line { 1867 | ZOrder 13 1868 | SrcBlock "From2" 1869 | SrcPort 1 1870 | DstBlock "To Workspace2" 1871 | DstPort 1 1872 | } 1873 | Line { 1874 | ZOrder 14 1875 | SrcBlock "From3" 1876 | SrcPort 1 1877 | DstBlock "To Workspace3" 1878 | DstPort 1 1879 | } 1880 | Line { 1881 | ZOrder 15 1882 | SrcBlock "From4" 1883 | SrcPort 1 1884 | DstBlock "To Workspace4" 1885 | DstPort 1 1886 | } 1887 | Line { 1888 | ZOrder 16 1889 | SrcBlock "From5" 1890 | SrcPort 1 1891 | DstBlock "To Workspace5" 1892 | DstPort 1 1893 | } 1894 | Line { 1895 | ZOrder 17 1896 | SrcBlock "traj4" 1897 | SrcPort 1 1898 | DstBlock "Demux1" 1899 | DstPort 1 1900 | } 1901 | } 1902 | } 1903 | -------------------------------------------------------------------------------- /traj/trajTest.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DaiLuyaoME/Auto-tuning-of-model-based-feedforward-controller-in-ultraprecision-motion-systems/05efbdbd0e853b2d2ef3a15581306367d41388a3/traj/trajTest.slx -------------------------------------------------------------------------------- /traj/trajTest.slx.r2016b: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DaiLuyaoME/Auto-tuning-of-model-based-feedforward-controller-in-ultraprecision-motion-systems/05efbdbd0e853b2d2ef3a15581306367d41388a3/traj/trajTest.slx.r2016b -------------------------------------------------------------------------------- /traj/trajTest.slx.r2017a: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DaiLuyaoME/Auto-tuning-of-model-based-feedforward-controller-in-ultraprecision-motion-systems/05efbdbd0e853b2d2ef3a15581306367d41388a3/traj/trajTest.slx.r2017a --------------------------------------------------------------------------------