diff --git a/examples/LinkShield/LinkShield_Identification/LinkShield_Identification.ino b/examples/LinkShield/LinkShield_Identification/LinkShield_Identification.ino index 5eaacf65..d02fd77e 100644 --- a/examples/LinkShield/LinkShield_Identification/LinkShield_Identification.ino +++ b/examples/LinkShield/LinkShield_Identification/LinkShield_Identification.ino @@ -27,7 +27,7 @@ bool endExperiment = false; // Boolean flag to end the experiment float y_1 = 0.0; // Output variable float y_2 = 0.0; // Output variable float u = 0.0; // Input (open-loop), initialized to zero -float U[]={0.00, -5.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 5.00, 0.00}; // Input trajectory +float U[]={0.00, 0.00, 5.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00}; // Input trajectory int T = 25; // Section length (appr. '/.+2 s) unsigned long int i = 0; // Section counter @@ -36,7 +36,7 @@ void setup() { // Initialize linkshield hardware LinkShield.begin(); // Define hardware pins - //LinkShield.calibrate(); // Remove sensor bias + LinkShield.calibrate(); // Remove sensor bias // Initialize sampling function Sampling.period(Ts * 1000); // Sampling init. @@ -89,9 +89,9 @@ void step(){ //Actuate with voltage converted to PWM duty cicle with square root and direction(+/-5V) LinkShield.actuatorWriteNew(u); - Serial.print(y_1,8); // Print outputs + Serial.print(y_2,4); // Print outputs Serial.print(", "); - Serial.print(y_2,8); // Print outputs + Serial.print(y_1,4); // Print outputs Serial.print(", "); Serial.println(u); // Print input diff --git a/examples/LinkShield/LinkShield_LQ/LinkShield_LQ.ino b/examples/LinkShield/LinkShield_LQ/LinkShield_LQ.ino index 589429e7..dbdb7c99 100644 --- a/examples/LinkShield/LinkShield_LQ/LinkShield_LQ.ino +++ b/examples/LinkShield/LinkShield_LQ/LinkShield_LQ.ino @@ -51,9 +51,8 @@ BLA::Matrix<4, 1> Xk = {0, 0, 0, 0}; BLA::Matrix<4, 1> xIC = {0, 0, 0, 0}; -//BLA::Matrix<1, 4> K = {6.0679, -15.234, 0.40104, -3.6504}; -BLA::Matrix<1, 4> K = {21.927, -116.4, 0.90889, -4.8342}; +BLA::Matrix<1, 4> K = {35.829, -173.02, 1.4342, -4.1063}; BLA::Matrix<4, 4> A = {1, 0.03235, 0.00475, 5e-05, 0, 0.9043, 6e-05, 0.00484, 0, 12.508, 0.90345, 0.03235, 0, -37.604, 0.02245, 0.9043}; BLA::Matrix<4, 1> B = {0.00068, -0.0004, 0.26726, -0.15689}; @@ -70,7 +69,7 @@ void setup() { // Initialize linkshield hardware LinkShield.begin(); // Define hardware pins - //LinkShield.calibrate(); // Remove sensor bias + LinkShield.calibrate(); // Remove sensor bias // Initialize sampling function Sampling.period(Ts *1000); // Sampling init. @@ -160,8 +159,8 @@ Serial.print(", "); Serial.print(X(0),4); Serial.print(", "); Serial.print(y_2,4); -//Serial.print(", "); -//Serial.println(X(1),4); +Serial.print(", "); +Serial.print(X(1),4); Serial.print(", "); Serial.print(Xr(0),4); Serial.print(", "); diff --git a/examples/LinkShield/LinkShield_OpenLoop/LinkShield_OpenLoop.ino b/examples/LinkShield/LinkShield_OpenLoop/LinkShield_OpenLoop.ino new file mode 100644 index 00000000..31489f29 --- /dev/null +++ b/examples/LinkShield/LinkShield_OpenLoop/LinkShield_OpenLoop.ino @@ -0,0 +1,174 @@ +/* + LinkShield identification example. + + Example used to acquire data for LinkShield system identification. + + The LinkShield implements an a flexible rotational link experiment + on an Arduino shield. This example initialises the sampling + subsystem from the AutomationShield library and allows user + + This code is part of the AutomationShield hardware and software + ecosystem. Visit http://www.automationshield.com for more + details. This code is licensed under a Creative Commons + Attribution-NonCommercial 4.0 International License. + + Created by Martin Vríčan. + Last update: 20.3.2023. +*/ + +#include // Include the library + + + +#define USE_DIFFERENTIATION 0 +#define USE_KALMAN 1 + +unsigned long Ts = 5; // Sampling in milliseconds +unsigned long k = 0; // Sample index +bool nextStep=false; // Flag for sampling +bool realTimeViolation = false; // Flag for real-time sampling violation +bool endExperiment = false; // Boolean flag to end the experiment + +float y_1 = 0.0; // Output variable +float y_2 = 0.0; // Output variable +float y_1prev = 0.0; +float y_2prev = 0.0; + + +float u = 0.0; // Input (open-loop), initialized to zero +float R[]={0.00,0.00, 0.00, 0.00}; // Input trajectory +int T = 5000; // Section length (appr. '/.+2 s) +unsigned int i = 0; // Section counter + + + + +BLA::Matrix<4, 1> X = {0, 0, 0, 0}; +BLA::Matrix<4, 1> Xr = {0, 0, 0, 0}; +BLA::Matrix<4, 1> U = {0, 0, 0, 0}; +BLA::Matrix<2, 1> Y = {0, 0}; +BLA::Matrix<4, 1> Xk = {0, 0, 0, 0}; +BLA::Matrix<4, 1> xIC = {0, 0, 0, 0}; + + +//BLA::Matrix<1, 4> K = {6.0679, -15.234, 0.40104, -3.6504}; +BLA::Matrix<1, 4> K = {19.689, -103.72, 0.8189, -4.9595}; + +BLA::Matrix<4, 4> A = {1, 0.03235, 0.00475, 5e-05, 0, 0.9043, 6e-05, 0.00484, 0, 12.508, 0.90345, 0.03235, 0, -37.604, 0.02245, 0.9043}; +BLA::Matrix<4, 1> B = {0.00068, -0.0004, 0.26726, -0.15689}; +BLA::Matrix<2, 4> C = {1, 0, 0, 0, 0, 1, 0, 0}; +BLA::Matrix<4, 4> Q_Kalman = {1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1}; +BLA::Matrix<2, 2> R_Kalman = {0.05, 0, 0.015, 0}; + + + + + +void setup() { + Serial.begin(250000); // Initialize serial + + // Initialize linkshield hardware + LinkShield.begin(); // Define hardware pins + //LinkShield.calibrate(); // Remove sensor bias + + // Initialize sampling function + Sampling.period(Ts *1000); // Sampling init. + Sampling.interrupt(stepEnable); // Interrupt fcn. +} + +// Main loop launches a single step at each enable time +void loop() { + if (nextStep) { // If ISR enables + step(); // Algorithm step + nextStep = false; // Then disable + } +} + +void stepEnable(){ // ISR + if(endExperiment == true){ // If the experiment is over + LinkShield.actuatorWriteNew(0.00); + while(1); // Do nothing + } + if(nextStep) { // If previous sample still running + realTimeViolation = true; // Real-time has been violated + Serial.println("Real-time samples violated."); // Print error message + while(1); // Stop program execution + } + nextStep=true; // Change flag +} + +// A single algorithm step +void step(){ + +// Switching between experiment sections + +if (i > sizeof(R) / sizeof(R[0])) { // If at end of trajectory + endExperiment = true; // Stop program execution at next ISR + } + else if (k % (T*i) == 0) { // If at the end of section + Xr(0) = R[i]; // Progress in trajectory + + i++; // Increment section counter + } + +//Xr(0) = LinkShield.referenceRead(); + +y_1 = LinkShield.servoPotRead(); // Read sensor +y_2 = LinkShield.flexRead(); + +Y(0) = y_1; +Y(1) = y_2; + +#if USE_DIFFERENTIATION +X(0) = y_1; +X(1) = y_2; +X(2) = (y_1 - y_1prev)/Ts; +X(3) = (y_2 - y_2prev)/Ts; +#endif + +#if USE_KALMAN +LinkShield.getKalmanEstimate(Xk, u, Y, A, B, C, Q_Kalman, R_Kalman, xIC); +X(0) = Xk(0); +X(1) = Xk(1); +X(2) = Xk(2); +X(3) = Xk(3); +#endif + + +U = Xr - X; +u = (K*U)(0); + + + if (y_1<-HALF_PI) {u = AutomationShield.constrainFloat(u, 0.0,5.0);} + else if (y_1>HALF_PI) {u = AutomationShield.constrainFloat(u,-5.0,0.0);} + else {u = AutomationShield.constrainFloat(u,-5.0,5.0);} + +#if USE_DIFFERENTIATION +y_1prev = y_1; +y_2prev = y_2; +#endif + + +//LinkShield.actuatorWritePWM(u); // Actuate +//LinkShield.actuatorWritePercent(u); +LinkShield.actuatorWriteNew(u); + + +Serial.print(y_1,4); // Print output +Serial.print(", "); +Serial.print(X(0),4); +Serial.print(", "); +Serial.print(y_2,4); +//Serial.print(", "); +//Serial.println(X(1),4); +Serial.print(", "); +Serial.print(Xr(0),4); +Serial.print(", "); +Serial.println(u); // Print input + +k++; // Sample counter +} + + + + diff --git a/examples/LinkShield/LinkShield_PID/LinkShield_PID.ino b/examples/LinkShield/LinkShield_PID/LinkShield_PID.ino index 3f8a908b..1ed1be9b 100644 --- a/examples/LinkShield/LinkShield_PID/LinkShield_PID.ino +++ b/examples/LinkShield/LinkShield_PID/LinkShield_PID.ino @@ -13,7 +13,7 @@ Attribution-NonCommercial 4.0 International License. Created by Martin Vríčan. - Last update: 3.12.2022. + Last update: 24.05.2023. */ #include // Include header for hardware API @@ -26,48 +26,58 @@ bool realTimeViolation = false; // Flag for real-time sampling violation bool endExperiment = false; // Boolean flag to end the experiment float y = 0.0; -float y1 = 0.0; // Output variable -float y2 = 0.0; // Output variable +float y_1 = 0.0; // Output variable +float y_2 = 0.0; // Output variable float r = 0.0; // Input (open-loop), initialized to zero float u = 0.0; -float R[]={0.00, PI/4, -PI/4, 0.00}; -int T = 1000; // Section length (appr. '/.+2 s) +float u_slow = 0.0; +float u_fast = 0.0; +float R[]={0.00, PI/6, -PI/6, 0.00}; +int T = 10; // Section length (appr. '/.+2 s) unsigned int i = 0; // Section counter // PID Tuning -#define KP 80 // PID Kp -#define KI 20.5 -#define KD 0.0001 +#define KP_Slow 85 +#define KI_Slow 30 +#define KD_Slow 1 -#define TI 0.1 // PID Ti -#define TD 0.15 // PID Td +#define VIBRATION_CONTROL 0 +#define KP_Fast 80 +#define KI_Fast 50 +#define KD_Fast 0.2 -void setup() { - Serial.begin(2000000); // Initialize serial +PIDAbsClass PIDAbsSlow; +PIDAbsClass PIDAbsFast; + +void setup() { + Serial.begin(250000); // Initialize serial // Initialize linkshield hardware LinkShield.begin(); // Define hardware pins - //LinkShield.calibrate(); // Remove sensor bias + LinkShield.calibrate(); // Remove sensor bias // Initialize sampling function Sampling.period(Ts * 1000); // Sampling init. Sampling.interrupt(stepEnable); // Interrupt fcn. // Set the PID constants - PIDAbs.setKp(KP); // Proportional - PIDAbs.setKi(KI); // Integral - PIDAbs.setKd(KD); // Derivative + PIDAbsSlow.setKp(KP_Slow); // Proportional + PIDAbsSlow.setKi(KI_Slow); // Integral + PIDAbsSlow.setKd(KD_Slow); // Derivative - //PIDAbs.setTi(TI); // Integral - //PIDAbs.setTd(TD); // Derivative - PIDAbs.setTs(Sampling.samplingPeriod); // Sampling + PIDAbsFast.setKp(KP_Fast); // Proportional + PIDAbsFast.setKi(KI_Fast); // Integral + PIDAbsFast.setKd(KD_Fast); // Derivative + + PIDAbsSlow.setTs(Sampling.samplingPeriod); // Sampling + PIDAbsFast.setTs(Sampling.samplingPeriod); // Sampling } // Main loop launches a single step at each enable time void loop() { - if (nextStep) { // If ISR enables + if (nextStep) { // If ISR enables step(); // Algorithm step nextStep = false; // Then disable } @@ -75,7 +85,7 @@ void loop() { void stepEnable() { // ISR if (endExperiment == true) { // If the experiment is over - LinkShield.actuatorWriteVoltage(0.00); + LinkShield.actuatorWriteNew(0.00); while (1); // Do nothing } if (nextStep == true) { // If previous sample still running @@ -100,23 +110,33 @@ void step() { i++; // Increment section counter } - y1 = LinkShield.encoderRead(); - //y2 = LinkShield.flexRead(); // Read sensor - - y = y1 + y2; - u = PIDAbs.compute((r - y), -5, 5, -100, 100); // Compute constrained absolute-form PID - - if(y>HALF_PI){u = AutomationShield.constrainFloat(u, -5.0,0.0);} - if(y<-HALF_PI){u = AutomationShield.constrainFloat(u,0.0,5.0);} - u = AutomationShield.constrainFloat(u,-5.0,5.0); + y_1 = LinkShield.servoPotRead(); + y_2 = LinkShield.flexRead(); // Read sensor - LinkShield.actuatorWriteVoltage(u); // [V] actuate + u_slow =PIDAbsSlow.compute((r - y_1), -5, 5, -30, 30); // Compute constrained absolute-form PID + + #if VIBRATION_CONTROL + u_fast =PIDAbsFast.compute((y_2), -5, 5, -100, 100); // Compute constrained absolute-form PID + u = AutomationShield.constrainFloat((u_slow+u_fast),-5.0,5.0); + #else + u = AutomationShield.constrainFloat((u_slow),-5.0,5.0); + #endif + + LinkShield.actuatorWriteNew(u); // [V] actuate // Print to serial port - Serial.print(r); // Print reference + Serial.print(r); Serial.print(", "); - Serial.print(y); // Print output + Serial.print(y_1,8); // Print reference + Serial.print(", "); + Serial.print(y_2,8); // Print output + Serial.print(", "); + Serial.print(u_slow); // Print output + Serial.print(", "); + Serial.print(u_fast); // Print output Serial.print(", "); Serial.println(u); // Print input + + k++; // Increment time-step k } diff --git a/matlab/examples/FloatShield/FloatShield_MPC.m b/matlab/examples/FloatShield/FloatShield_MPC.m index eb519b8e..8deb0e48 100644 --- a/matlab/examples/FloatShield/FloatShield_MPC.m +++ b/matlab/examples/FloatShield/FloatShield_MPC.m @@ -17,12 +17,12 @@ % Created by Peter Chmurciak. % Last update: 24.4.2020. -startScript; % Clears screen and variables, except allows CI testing -clear estimateKalmanState; % Clears persistent variables in estimate function - -FloatShield = FloatShield; % Create FloatShield object from FloatShield class -FloatShield.begin('COM4', 'UNO'); % Initialise shield with used Port and Board type -FloatShield.calibrate(); % Calibrate FloatShield +% startScript; % Clears screen and variables, except allows CI testing +% clear estimateKalmanState; % Clears persistent variables in estimate function +% +% FloatShield = FloatShield; % Create FloatShield object from FloatShield class +% FloatShield.begin('COM4', 'UNO'); % Initialise shield with used Port and Board type +% FloatShield.calibrate(); % Calibrate FloatShield Ts = 0.025; % Sampling period in seconds k = 1; % Algorithm step counter diff --git a/matlab/examples/LinkShield/LinkShield_Identification_SS.m b/matlab/examples/LinkShield/LinkShield_Identification_SS.m index 87403726..4243f6f4 100644 --- a/matlab/examples/LinkShield/LinkShield_Identification_SS.m +++ b/matlab/examples/LinkShield/LinkShield_Identification_SS.m @@ -137,6 +137,7 @@ Q_Kalman = diag([50 20 1 1]); + for i=1:length(Uval) % Y(1,1) = Theta(i); @@ -146,18 +147,43 @@ x(:,i+1) = dA*x(:,i) + dB*Uval(i); y(:,i+1) = dC*x(:,i+1); +tt(:,i) = i*Ts-Ts; +end + + +figure() +subplot(2,1,1) +plot(tt,y(1,2:278)) +hold on +plot(tt',Theta) +legend("Simulované dáta", "Merané dáta","Location","southeast") +xticks(0:0.25:1.5) +ylim([-0.2 1.7]) +grid on +ylabel("Theta (Rad)") +subplot(2,1,2) +plot(tt,y(2,2:278)) +hold on +plot(tt',Alpha) +legend("Simulované dáta", "Merané dáta","Location","southeast") +xticks(0:0.25:1.5) +ylim([-0.1 0.05]) +grid on +ylabel('Alpha (Rad)') +xlabel ("Čas (s)") + + + - -end %% -tplot = 0:5:299*5; +t = 0:5:299*5; tplot = t/1000; - +load LinkShield_ID_Data.mat subplot(3,1,1) -stairs(t,U,LineWidth=1.5) +stairs(tplot,U,LineWidth=1.5) legend("U") title("Priebeh identifikačného experimentu") xticks(0:0.25:1.5) @@ -165,14 +191,14 @@ grid on ylabel("Napätie (V)") subplot(3,1,2) -plot(t,Theta,LineWidth=1.5) +plot(tplot,Theta,LineWidth=1.5) legend("Theta","Location","southeast") xticks(0:0.25:1.5) ylim([-0.2 1.7]) grid on ylabel("Uhol (Rad)") subplot(3,1,3) -plot(t,Alpha,LineWidth=1.5) +plot(tplot,Alpha,LineWidth=1.5) legend("Alpha","Location","southeast") xticks(0:0.25:1.5) ylim([-0.1 0.05]) diff --git a/matlab/examples/LinkShield/LinkShield_LQ_OpenLoop_calculation.m b/matlab/examples/LinkShield/LinkShield_LQ_OpenLoop_calculation.m new file mode 100644 index 00000000..0d3a99f2 --- /dev/null +++ b/matlab/examples/LinkShield/LinkShield_LQ_OpenLoop_calculation.m @@ -0,0 +1,167 @@ +clear; close all; clc + +%% Load identified state space model +load LinkShield_SSID.mat + +R = [0.0, pi/2, -pi/4]; +T = 1000; +i = 1; + +t=0:Ts:(T*length(R)-1)*Ts; + +r=[R(1);0;0;0]; + + +integrator = 0; + +%% Discretize model +A = model.A; +B = model.B; +C = model.C; +D = model.D; + +SYSC = ss(A,B,C,D); +SYSD = c2d(SYSC,Ts); + +AD = SYSD.A; +BD = SYSD.B; +CD = SYSD.C; +DD = SYSD.D; + +%% Kalman noise covariance matrices +R_Kalman = [5.808465952461661e-03 0; + 0 1.266009518986769e-07]; + +Q_Kalman = diag([250 100 1 100]); + +%% Create integrator states matrices + +[ny, ~ ]=size(C); +[nx, nu]=size(B); + +Ai=[eye(ny,ny) -CD; % Augmenting A by an integrator + zeros(nx,ny) AD]; +Bi=[zeros(ny,nu); BD]; % Augmenting B by the integrator +Ci=[zeros(ny,nx) CD]; % Augmenting C by the integrator + +%% LQ gain calculation + +if integrator +Rlq= 1e-4; +%Qlq=diag([1 1.2 0 0 0 0]); +Qlq=diag([1e-2 1e-10 1e2 1e3 1e-2 1e-2]); + +[K, P] = dlqr(Ai, Bi, Qlq, Rlq); +else +Rlq = 1e-2; +%Qlq = diag([1e0 3e2 1e-15 1e-15]); +Qlq = diag([1250 1 1 95]); + + +[K, P] = dlqr(AD, BD, Qlq, Rlq); +end + +%% Simulation + +x0 = [0 0 0 0]'; +X = x0; +xI = [0 0]'; +y = [0 0]'; + +x1 = 0; +x2 = 0; +x1p = 0; +x2p = 0; + +umin = -5; +umax = 5; + +Y = zeros(2,2); +U = zeros(1,length(t)); + +if integrator +for k = 1:length(t)-1 + + if (mod(k,(T*i)) == 0) % At each section change + i = i + 1; % We move to the next index + r = [R(i); 0]; % and use the actual, corrected reference + end + + % state estimate by difference + x1 = y(1,k)-r(1); + x2 = y(2,k)-r(2); + x3 = (x1 - x1p)/Ts; + x4 = (x2 - x2p)/Ts; + x1p = x1; + x2p = x2; + + xhat = [x1; x2; x3; x4]; + + xI(:,k+1) = xI(:,k)+(r-[x1; x2]); % Integrator state + U(k+1) = -K*[xI(:,k); xhat]; % LQ compensation for the integrator augmented linear part + U(k+1) = constrain(U(k+1),umin,umax); % Constrain inputs like on the real system + + % Model + X(:,k+1)=AD*X(:,k)+BD*U(k+1); % System model + y(:,k+1)=CD * X(:,k+1) ; % Position measurement on the real system + + +end + +else +for k = 1:length(t)-1 + + if (mod(k,(T*i)) == 0) % At each section change + i = i + 1; % We move to the next index + r = [R(i); 0; 0; 0]; % and use the actual, corrected reference + end + + x1 = y(1,k); + x2 = y(2,k); + x3 = (x1 - x1p)/Ts; + x4 = (x2 - x2p)/Ts; + x1p = x1; + x2p = x2; + + xhat = [x1; x2; x3; x4]; + + u = r - xhat; + + U(k) = K*u; + U(k) = constrain(U(k),umin,umax); + x(:,k+1) = AD*xhat+BD*U(k); + y(:,k+1) = CD*x(:,k+1); + rplot(k)= r(1,1); +end +end + +%% Plotting results + +figure('Name','Simulacia') + + +subplot(3,1,1) +plot(y(2,:),'DisplayName','y2') +ylabel("Uhol (Rad)") +title("Alpha") + +subplot(3,1,2) +plot(y(1,:),'DisplayName','y1') +hold on +title("Theta, R") +plot(rplot) +ylabel("Uhol (Rad)") +subplot(3,1,3) +plot(U) +title("U") +ylabel("Napätie (V)") + +%% Print matrices in C/C++ format + +printSSMatrix(K,'K') + +printSSMatrix(AD,'A') +printSSMatrix(BD,'B') +printSSMatrix(CD,'C') +printSSMatrix(Q_Kalman,'Q_Kalman') +printSSMatrix(R_Kalman,'R_Kalman') diff --git a/matlab/examples/LinkShield/LinkShield_LQ_calculation.m b/matlab/examples/LinkShield/LinkShield_LQ_calculation.m index 743f66cf..3b32dfe1 100644 --- a/matlab/examples/LinkShield/LinkShield_LQ_calculation.m +++ b/matlab/examples/LinkShield/LinkShield_LQ_calculation.m @@ -3,7 +3,7 @@ %% Load identified state space model load LinkShield_SSID.mat -R = [0.0, pi/2, -pi/4]; +R = [0.0, pi/2, -pi/4, 0.0]; T = 1000; i = 1; @@ -31,9 +31,11 @@ %% Kalman noise covariance matrices R_Kalman = [5.808465952461661e-03 0; 0 1.266009518986769e-07]; - +if integrator +Q_Kalman = diag([1 1 250 100 1 100]); +else Q_Kalman = diag([250 100 1 100]); - +end %% Create integrator states matrices [ny, ~ ]=size(C); @@ -47,15 +49,15 @@ %% LQ gain calculation if integrator -Rlq= 1e-4; +Rlq= 1e-5; %Qlq=diag([1 1.2 0 0 0 0]); Qlq=diag([1e-2 1e-10 1e2 1e3 1e-2 1e-2]); [K, P] = dlqr(Ai, Bi, Qlq, Rlq); else -Rlq = 1e-2; +Rlq = 1e-3; %Qlq = diag([1e0 3e2 1e-15 1e-15]); -Qlq = diag([1250 1 1 75]); +Qlq = diag([1250 1 1 25]); [K, P] = dlqr(AD, BD, Qlq, Rlq); @@ -75,6 +77,8 @@ umin = -5; umax = 5; +TT = 0; + rplot = 0; Y = zeros(2,2); U = zeros(1,length(t)); @@ -131,7 +135,8 @@ U(k) = constrain(U(k),umin,umax); x(:,k+1) = AD*xhat+BD*U(k); y(:,k+1) = CD*x(:,k+1); - rplot(k)= r(1,1); + rplot(k+1)= r(1,1); + TT(k+1)=(k)/1000; end end @@ -140,28 +145,47 @@ figure('Name','Simulacia') + + subplot(3,1,1) -plot(y(2,:),'DisplayName','y2') -ylabel("Uhol (Rad)") -title("Alpha") +plot(TT,y(1,:),'DisplayName','y1') +hold on +plot(TT,rplot) +legend("Meranie","Referencia") +ylabel("Theta (Rad)") +grid on subplot(3,1,2) -plot(y(1,:),'DisplayName','y1') -hold on -title("Theta, R") -plot(rplot) -ylabel("Uhol (Rad)") +plot(TT,y(2,:),'DisplayName','y2') +ylabel("Alpha (Rad)") +%legend("Alpha") +grid on + + subplot(3,1,3) -plot(U) -title("U") +plot(TT,U) +xlabel("Čas (s)") ylabel("Napätie (V)") +%legend("U") +grid on %% Print matrices in C/C++ format + +if integrator + printSSMatrix(K,'K') +printSSMatrix(Ai,'A') +printSSMatrix(Bi,'B') +printSSMatrix(Ci,'C') +printSSMatrix(Q_Kalman,'Q_Kalman') +printSSMatrix(R_Kalman,'R_Kalman') +else +printSSMatrix(K,'K') printSSMatrix(AD,'A') printSSMatrix(BD,'B') printSSMatrix(CD,'C') printSSMatrix(Q_Kalman,'Q_Kalman') printSSMatrix(R_Kalman,'R_Kalman') +end \ No newline at end of file diff --git a/matlab/examples/LinkShield/LinkShield_PID.bin b/matlab/examples/LinkShield/LinkShield_PID.bin new file mode 100644 index 00000000..efb83273 Binary files /dev/null and b/matlab/examples/LinkShield/LinkShield_PID.bin differ diff --git a/matlab/examples/LinkShield/LinkShield_SSID.mat b/matlab/examples/LinkShield/LinkShield_SSID.mat index b7842fb7..b55b5438 100644 Binary files a/matlab/examples/LinkShield/LinkShield_SSID.mat and b/matlab/examples/LinkShield/LinkShield_SSID.mat differ diff --git a/simulink/LinkLibrary.slx b/simulink/LinkLibrary.slx index 58a1acf5..4b8f8d06 100644 Binary files a/simulink/LinkLibrary.slx and b/simulink/LinkLibrary.slx differ diff --git a/simulink/assets/LinkFig/LinkPotentiometer.jpg b/simulink/assets/LinkFig/LinkPotentiometer.jpg new file mode 100644 index 00000000..57d246da Binary files /dev/null and b/simulink/assets/LinkFig/LinkPotentiometer.jpg differ diff --git a/simulink/assets/LinkFig/LinkShieldISO2.jpg b/simulink/assets/LinkFig/LinkShieldISO2.jpg new file mode 100644 index 00000000..cf352284 Binary files /dev/null and b/simulink/assets/LinkFig/LinkShieldISO2.jpg differ diff --git a/simulink/assets/LinkFig/linkR3.png b/simulink/assets/LinkFig/linkR3.png new file mode 100644 index 00000000..be86a9d0 Binary files /dev/null and b/simulink/assets/LinkFig/linkR3.png differ diff --git a/simulink/examples/LinkShield/LinkShield_InputsOutputs.slx b/simulink/examples/LinkShield/LinkShield_InputsOutputs.slx new file mode 100644 index 00000000..9ea9d488 Binary files /dev/null and b/simulink/examples/LinkShield/LinkShield_InputsOutputs.slx differ diff --git a/simulink/examples/LinkShield/LinkShield_PID.slx b/simulink/examples/LinkShield/LinkShield_PID.slx new file mode 100644 index 00000000..c7d7078b Binary files /dev/null and b/simulink/examples/LinkShield/LinkShield_PID.slx differ diff --git a/src/AutomationShield.cpp b/src/AutomationShield.cpp index a0876252..16ac87cf 100644 --- a/src/AutomationShield.cpp +++ b/src/AutomationShield.cpp @@ -33,6 +33,12 @@ byte AutomationShieldClass::percToPwm(float perc){ float percFloat = perc*2.55; return byte(percFloat); } + +// Turns a floating point input in voltage to 8-bit PWM +byte AutomationShieldClass::voltToPwm(float Vin, float Vmax){ + float voltFloat = sq(Vin) * 255 / sq(Vmax); + return byte (voltFloat); +} void AutomationShieldClass::error(const char *str) // Error handler function { diff --git a/src/AutomationShield.h b/src/AutomationShield.h index c2440d90..b0b0beaa 100644 --- a/src/AutomationShield.h +++ b/src/AutomationShield.h @@ -50,6 +50,7 @@ void error(const char *str); float constrainFloat(float x, float min_x, float max_x); byte percToPwm(float perc); + byte voltToPwm(float Vin, float Vmax); float quality(float, char *method); // Quality metric for feedback control input // Printing functions diff --git a/src/LinkShield.h b/src/LinkShield.h index 2d51846d..b2370c01 100644 --- a/src/LinkShield.h +++ b/src/LinkShield.h @@ -140,10 +140,10 @@ void LinkClass::begin() { Wire.begin(); // Starts the "Wire" library for I2C analogReference(EXTERNAL); // Set reference voltage (3v3) #elif ARDUINO_ARCH_SAM - //analogReadResolution(12); + analogReadResolution(12); Wire1.begin(); // Initialize I2C communication #elif ARDUINO_ARCH_SAMD - //analogReadResolution(12); + analogReadResolution(12); Wire.begin(); // Initialize I2C communication #endif @@ -182,7 +182,7 @@ void LinkClass::isrB() { void LinkClass::calibrate() { - AutomationShield.serialPrint("Calibration..."); + //AutomationShield.serialPrint("Calibration..."); // Go to middle and wait delay(500); _zeroBendValue = flexBias(1000); @@ -194,7 +194,7 @@ void LinkClass::calibrate() { /* _sensorBias = -(LinkShield.sensorBias(1000)/4); // Calculate offset to LSB/g ADXL345_OFSZ(); */ - AutomationShield.serialPrint(" successful.\n"); + //AutomationShield.serialPrint(" successful.\n"); } @@ -202,26 +202,30 @@ void LinkClass::calibrate() { float LinkClass::flexBias(int testLength) { for (int i = 0; i < testLength; i++ ) { //Make N measurements - _flexSum = _flexSum + flexRead(); + _flexSum = _flexSum + analogRead(1); } - return _flexSum / testLength; //Compute average + float _flexMidBias = (ADCREF/2) - (_flexSum / testLength); + + return _flexMidBias; //Compute average } //values from potentiometer in degrees , for fututre use float LinkClass::referenceRead() { _referenceRead = analogRead(LINK_RPIN); - _referenceValue = AutomationShield.mapFloat((float)_referenceRead, 0.00, 1023.00, -90.00, 90.00)*DEG_TO_RAD; + _referenceValue = AutomationShield.mapFloat((float)_referenceRead, 0.00, ADCREF, -90.00, 90.00)*DEG_TO_RAD; return _referenceValue; } float LinkClass::flexRead() { _flexRead = analogRead(LINK_FLEX_PIN); - _flexValue = AutomationShield.mapFloat((float)_flexRead, 500.00, 550.00, 0.00, 2.50)*DEG_TO_RAD/10; + if(calibrated) { - _flexValue =_flexValue - _zeroBendValue; + _flexRead =_flexRead + _zeroBendValue; } - return _flexValue; + + _flexValue = AutomationShield.mapFloat((float)_flexRead, 0, ADCREF, -25.0, 25.0)*DEG_TO_RAD/10; + return - _flexValue; } #if ENCODER @@ -233,7 +237,7 @@ float LinkClass::encoderRead() { float LinkClass::servoPotRead() { _potValue = analogRead(SERVO_POT_PIN); - _angularValue = AutomationShield.mapFloat(_potValue,100.00,1000.00,-HALF_PI,HALF_PI); + _angularValue = AutomationShield.mapFloat(_potValue,0,ADCREF,-110,110)*DEG_TO_RAD; return _angularValue; }