Upload latest version

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
 

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(", ");

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 <LinkShield.h>               // 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
+}
+
+
+
+

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 <LinkShield.h>            	// Include header for hardware API
@@ -26,56 +26,66 @@ 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
   }
 }
 
 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
 }

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