Skip to content

Commit

Permalink
Merge pull request #372 from gergelytakacs/BicopShield
Browse files Browse the repository at this point in the history 
BicopShield
  • Loading branch information
@gergelytakacs
gergelytakacs authored Jun 4, 2024
2 parents a561961 + 46faef5 commit bc0778f
Show file tree
Hide file tree
Showing 34 changed files with 53,862 additions and 0 deletions.
152 changes: 152 additions & 0 deletions examples/BicopShield/BicopShield_EMPC/BicopShield_EMPC.ino
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,152 @@
/*
BicopShield closed-loop eMPC control example
Explicit Model Predictive Control (eMPC).
This example initialises the sampling subsystem from
the AutomationShield library and then realises eMPC control of
the angle of the arm. MPC control is implemented
by using the MPT3 software, see BicopShield_Export_EMPC.m
for the problem definition. The example allows the user to select
whether the reference altitude is given by the potentiometer or by
a predetermined reference trajectory. Upload the code to your board
and open the Serial Plotter tool in Arduino IDE.
Tested on an UNO(max prediction horizon 1).
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 Nemček.
Created on: 6.5.2024
Last update: 24.5.2024
*/

#include <BicopShield.h> // Include main library
#include <Sampling.h> // Include sampling library

#include "ectrl.h" // Include header file for EMPC controller

#include <empcSequential.h>

#define MANUAL 0 // Choose manual reference using potentiometer (1) or automatic reference trajectory (0)

#define USE_KALMAN 0 // (1) use Kalman filter for state estimation, (0) use direct measurement and simple difference for speed

#define Ts 10.0 // Sampling period in milliseconds
unsigned long k = 0; // Sample index
bool nextStep = false; // Flag for step function
bool realTimeViolation = false; // Flag for real-time sampling violation

float R[]={0.8,1.0,0.35,1.3,1.0,0.35,0.8,1.2,0.0}; // Reference trajectory
float y = 0.0; // [rad] Output (Current object height)
float yprev= 0.0; // [rad] Previous output (Object height in previous sample)
float u1,u2;
int T = 1000; // Section length
int i = 0; // Section counter


// Kalman filter
#if USE_KALMAN
BLA::Matrix<2, 2> A = {0.99999, 0.00998, -0.00246, 0.99525};
BLA::Matrix<2, 2> B = {7.78747048702292e-06, -9.62042244147616e-06, 0.00156, -0.00192};
BLA::Matrix<1, 2> C = {1, 0};
BLA::Matrix<2, 2> Q_Kalman = {1, 0, 0, 100};
BLA::Matrix<1, 1> R_Kalman = {0.01};
#endif

float X[3] = {0.0, 0.0, 0.0}; // Estimated initial state vector
float Xr[3] = {0.0, 0.0, 0.0}; // Initial reference state vector
//float Xk[2] = {0.0, 0.0};

float u = 0.0; // [V] Input (Magnet voltage)
static float u_opt[MPT_RANGE]; // predicted inputs
float BaseU=50.0; // Base input
float ref_read; // Variable for potentiometer
extern struct mpc_ctl ctl; // Object representing presets for MPC controller

void setup() {
Serial.begin(115200); //--Initialize serial communication
BicopShield.begin(); //--Initialize AeroShield
delay(1000);
BicopShield.calibrate(); // Calibrate AeroShield board + store the 0° value of the pendulum
Serial.println("r, y, u"); //--Print header
Sampling.period(Ts*1000.0);
Sampling.interrupt(stepEnable);
}

void loop(){
if (nextStep) { // Running the algorithm once every sample
step();
nextStep = false; // Setting the flag to false # built-in ISR sets flag to true at the end of each sample
}
}

void stepEnable() { // ISR
// if (nextStep) { // If previous sample still running
// realTimeViolation = true; // Real-time has been violated
// noInterrupts();
// Serial.println("Real-time samples violated."); // Print error message
// BicopShield.actuatorWrite(0.0,0.0); // Turn off the fan
// while (1); // Stop program execution
//}
nextStep = true; // Enable step flag
}

void step() { // Define step function
#if MANUAL
ref_read=BicopShield.referenceRead();
Xr[1] = AutomationShield.mapFloat(ref_read,0,100,0,100); //--Sensing Pot reference
#elif !MANUAL
if(i >= sizeof(R)/sizeof(float)){ // If trajectory ended
BicopShield.actuatorWriteVolt(0.0,0.0); // Stop the Motor
while(true); // End of program execution
}
if (k % (T*i) == 0){ // Moving through trajectory values
//r = R[i];
Xr[1] = R[i];
i++; // Change input value after defined amount of samples
}
k++; // Increment
#endif

y = BicopShield.sensorReadRadian(); // Angle in radians

#if USE_KALMAN
BLA::Matrix<2, 1> Xk = {X[1], X[2]};
BLA::Matrix<2, 1> U = {u_opt[0], u_opt[1]};
BicopShield.getKalmanEstimate(Xk, U, y, A, B, C, Q_Kalman, R_Kalman); // State estimation using Kalman filter
X[1] = Xk(0);
X[2] = Xk(1);
X[0] = X[0] + (Xr[1]- X[1]);
#else
// Direct angle and simple difference for Angular speed
// Saving valuable milliseconds for MPC algorithm

X[1] = y; // Arm angle
X[2] = (y-yprev)/(float(Ts)/1000.0); // Angular speed of the arm
#endif

X[0] = X[0] + (Xr[1] - X[1]); // Integral state
yprev=y;

empcSequential(X, u_opt); // solve Explicit MPC problem
u1 =BaseU+u_opt[0]; // Save system input into input variable
u2 =BaseU+u_opt[1];

BicopShield.actuatorWrite(u1,u2); // Actuation

Serial.print(Xr[1],3); // Printing reference
Serial.print(", ");
Serial.print(X[1],3); // Printing output
Serial.print(", ");
Serial.print(X[2],4);
Serial.print(", ");
Serial.print(u1,3); // Printing input
Serial.print(", ");
Serial.print(u2,3); // Printing input
Serial.print(", ");
Serial.println(X[1],4);
}
98 changes: 98 additions & 0 deletions examples/BicopShield/BicopShield_EMPC/ectrl.h
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,98 @@
/*
MATRICES FOR EXPLICIT MPC POLYTOPES AND CORRESPONDING PWA LAWS
The automatically generated set of matrices have been adapted from
the C code auto-generation routine of the Multi-Parametric Toolbox 3
MPT3) of Kvasnica et al. for MATLAB. Please see http://www.mpt3.org
for more information. Please see the original "toC.m" for more details
in the MPT3.
The following C code has been slightly adapted to perform microcontroller
architecutre-specific changes for AVR and ARM Cortex-A devices.
The list of changes from the original are as follows:
- Changed "static" modifiers to "const" so microcontrollers will
prefer to use ROM instead of RAM.
- Added PROGMEM functionality for the AVR architecture, and calling
the necessary headers.
- Removed the possibility to use PWQ, this only handles PWA.
- Removed matrix export for the tie-breaking function.
Usage: include alongside with the empcSequential.h header to your
example.
This code snippet has been altered for the needs of the
the AutomationShield hardware and software ecosystem.
Visit http://www.automationshield.com for more details.
Copyright (C) 2005 by Michal Kvasnica ([email protected])
Revised in 2012-2013 by Martin Herceg ([email protected])
Adapted for MCU use by Gergely Tak�cs in 2020 ([email protected])
*/
#include <avr/pgmspace.h>

#define MPT_NR 5
#define MPT_DOMAIN 3
#define MPT_RANGE 2
#define MPT_ABSTOL 1.000000e-08

const float MPT_A[] PROGMEM = {
2.9635436e-02, -9.8775575e-01, -1.5316764e-01, -7.0710678e-01, 7.0710678e-01,
0.0000000e+00, 7.0710678e-01, -7.0710678e-01, 0.0000000e+00, -1.0000000e+00,
0.0000000e+00, 0.0000000e+00, 0.0000000e+00, 0.0000000e+00, -1.0000000e+00,
1.0000000e+00, 0.0000000e+00, 0.0000000e+00, 0.0000000e+00, 0.0000000e+00,
1.0000000e+00, -2.9635436e-02, 9.8775575e-01, 1.5316764e-01, -7.0710678e-01,
7.0710678e-01, 0.0000000e+00, 7.0710678e-01, -7.0710678e-01, 0.0000000e+00,
-1.0000000e+00, 0.0000000e+00, 0.0000000e+00, 0.0000000e+00, 0.0000000e+00,
-1.0000000e+00, 1.0000000e+00, 0.0000000e+00, 0.0000000e+00, 0.0000000e+00,
0.0000000e+00, 1.0000000e+00, 2.9635436e-02, -9.8775575e-01, -1.5316764e-01,
-2.9635436e-02, 9.8775575e-01, 1.5316764e-01, 2.9635436e-02, -9.8775575e-01,
-1.5316764e-01, -2.9635436e-02, 9.8775575e-01, 1.5316764e-01, -7.0710678e-01,
7.0710678e-01, 0.0000000e+00, 7.0710678e-01, -7.0710678e-01, 0.0000000e+00,
-1.0000000e+00, 0.0000000e+00, 0.0000000e+00, 0.0000000e+00, 0.0000000e+00,
-1.0000000e+00, 1.0000000e+00, 0.0000000e+00, 0.0000000e+00, 0.0000000e+00,
0.0000000e+00, 1.0000000e+00, -2.9635436e-02, 9.8775575e-01, 1.5316764e-01,
-7.0710678e-01, 7.0710678e-01, 0.0000000e+00, 0.0000000e+00, -9.9995024e-01,
-9.9758962e-03, 0.0000000e+00, -1.0000000e+00, 0.0000000e+00, 7.0710678e-01,
-7.0710678e-01, 0.0000000e+00, -1.0000000e+00, 0.0000000e+00, 0.0000000e+00,
0.0000000e+00, 0.0000000e+00, -1.0000000e+00, 1.0000000e+00, 0.0000000e+00,
0.0000000e+00, 0.0000000e+00, 0.0000000e+00, 1.0000000e+00, -7.0710678e-01,
7.0710678e-01, 0.0000000e+00, 7.0710678e-01, -7.0710678e-01, 0.0000000e+00,
0.0000000e+00, 9.9995024e-01, 9.9758962e-03, 0.0000000e+00, 1.0000000e+00,
0.0000000e+00, -1.0000000e+00, 0.0000000e+00, 0.0000000e+00, 0.0000000e+00,
0.0000000e+00, -1.0000000e+00, 1.0000000e+00, 0.0000000e+00, 0.0000000e+00,
0.0000000e+00, 0.0000000e+00, 1.0000000e+00, 2.9635436e-02, -9.8775575e-01,
-1.5316764e-01 };

const float MPT_B[] PROGMEM = {
1.2172140e-01, 7.0710678e+03, 7.0710678e+03, 1.0000000e+04, 1.0000000e+04,
1.0000000e+04, 1.0000000e+04, 1.2172140e-01, 7.0710678e+03, 7.0710678e+03,
1.0000000e+04, 1.0000000e+04, 1.0000000e+04, 1.0000000e+04, 1.4834728e-01,
-1.2172140e-01, -1.2172140e-01, 1.4834728e-01, 7.0710678e+03, 7.0710678e+03,
1.0000000e+04, 1.0000000e+04, 1.0000000e+04, 1.0000000e+04, -1.4834728e-01,
7.0710678e+03, 9.9996259e+03, 1.0000000e+04, 7.0710678e+03, 1.0000000e+04,
1.0000000e+04, 1.0000000e+04, 1.0000000e+04, 7.0710678e+03, 7.0710678e+03,
9.9996259e+03, 1.0000000e+04, 1.0000000e+04, 1.0000000e+04, 1.0000000e+04,
1.0000000e+04, -1.4834728e-01 };

const int MPT_NC[] PROGMEM = {
8, 8, 8, 9, 9
};


const float MPT_F[] PROGMEM = {
5.9124557e+00, -1.9706348e+02, -3.0557908e+01, -7.3040818e+00, 2.4344669e+02,
3.7750381e+01, 6.3618820e+00, -2.1204296e+02, -3.2880720e+01, 0.0000000e+00,
0.0000000e+00, 0.0000000e+00, 6.3618820e+00, -2.1204296e+02, -3.2880720e+01,
0.0000000e+00, 0.0000000e+00, 0.0000000e+00, 0.0000000e+00, 0.0000000e+00,
0.0000000e+00, 0.0000000e+00, 0.0000000e+00, 0.0000000e+00, 0.0000000e+00,
0.0000000e+00, 0.0000000e+00, 0.0000000e+00, 0.0000000e+00, 0.0000000e+00
};


const float MPT_G[] PROGMEM = {
-3.5527137e-15, 0.0000000e+00, -1.8459251e+00, -3.0000000e+01, 1.8459251e+00,
3.0000000e+01, 3.0000000e+01, -3.0000000e+01, -3.0000000e+01, 3.0000000e+01
};

Loading

0 comments on commit bc0778f

Please sign in to comment.