Skip to content

Commit

Permalink
LQ tune
Browse files Browse the repository at this point in the history
  • Loading branch information
@martinvrican
martinvrican committed May 23, 2023
1 parent bb0aa6a commit d6e36a5
Show file tree
Hide file tree
Showing 6 changed files with 64 additions and 63 deletions.
4 changes: 2 additions & 2 deletions examples/LinkShield/LinkShield_Identification/LinkShield_Identification.ino
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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, 0.00, 0.00}; // Input trajectory
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
int T = 25; // Section length (appr. '/.+2 s)
unsigned long int i = 0; // Section counter

Expand Down Expand Up @@ -87,7 +87,7 @@ void step(){
//LinkShield.actuatorWritePercent(u); // Actuate with percent of PWM duty cicle

//Actuate with voltage converted to PWM duty cicle with square root and direction(+/-5V)
LinkShield.actuatorWrite(u);
LinkShield.actuatorWriteNew(u);

Serial.print(y_1,8); // Print outputs
Serial.print(", ");
Expand Down
18 changes: 12 additions & 6 deletions examples/LinkShield/LinkShield_LQ/LinkShield_LQ.ino
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,7 +51,10 @@ 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*4, -15.234/10, 0.40104, -3.6504/5};
//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<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};
Expand Down Expand Up @@ -84,7 +87,7 @@ void loop() {

void stepEnable(){ // ISR
if(endExperiment == true){ // If the experiment is over
LinkShield.actuatorWrite(0.00);
LinkShield.actuatorWriteNew(0.00);
while(1); // Do nothing
}
if(nextStep) { // If previous sample still running
Expand All @@ -104,10 +107,13 @@ 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
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();

Expand Down Expand Up @@ -146,13 +152,13 @@ y_2prev = y_2;

//LinkShield.actuatorWritePWM(u); // Actuate
//LinkShield.actuatorWritePercent(u);
LinkShield.actuatorWrite(u);
LinkShield.actuatorWriteNew(u);


Serial.print(y_1,4); // Print output
Serial.print(", ");
//Serial.print(X(0),4);
//Serial.print(", "); */
Serial.print(X(0),4);
Serial.print(", ");
Serial.print(y_2,4);
//Serial.print(", ");
//Serial.println(X(1),4);
Expand Down
17 changes: 9 additions & 8 deletions matlab/examples/LinkShield/LinkShield_Identification_TF.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,23 +22,24 @@
startScript; % Clears screen and variables, except allows CI testing

%% Data preprocessing
load resultID.mat % Read identificaiton results
Ts=0.003; % Sampling
data=iddata(y,u,Ts) % Create identification data object
load LinkShield_ID_Data.mat % Read identificaiton results
Ts=0.005; % Sampling
data=iddata(Alpha,U,Ts) % Create identification data object

% Select an appropriate section
dataS = data([1992:3971]); % Select data section (S)
dataSR = dataS([136:1920]); % Refine selection (R)
%dataS = data([1992:3971]); % Select data section (S)
%dataSR = dataS([136:1920]); % Refine selection (R)

%% Transfer function estimation

% Set up identification
Options = tfestOptions; % Create an options object
Options.Display = 'on'; % Display procedure
Options.WeightingFilter = []; % No weighting filter
Options.InitialState = 'zero';

% Launch and save identification
sys = tfest(dataSR, 2, 0, Options) % Identify transfer function
sys = tfest(data, 2, 0, Options) % Identify transfer function
save sys sys % Save transfer function

%% Data and display
Expand All @@ -50,6 +51,6 @@
c=sys.Numerator(1)/(-omega^2) % Actuator constant

% Comparison
compare(sys,dataS) % Comparison of data to model
compare(sys,data) % Comparison of data to model
xlabel('Time') % X axis label
ylabel('Accel. y(t) ms^{-2}') % Y axis label
ylabel('Angle (rad)') % Y axis label
6 changes: 3 additions & 3 deletions matlab/examples/LinkShield/LinkShield_LQ_calculation.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,15 +47,15 @@
%% LQ gain calculation

if integrator
Rlq= 1e-2;
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 = 1;
Rlq = 1e-2;
%Qlq = diag([1e0 3e2 1e-15 1e-15]);
Qlq = diag([125 1 1 50]);
Qlq = diag([1250 1 1 75]);


[K, P] = dlqr(AD, BD, Qlq, Rlq);
Expand Down
Binary file modified matlab/examples/LinkShield/sys.mat
View file
Binary file not shown.
82 changes: 38 additions & 44 deletions src/LinkShield.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,14 +29,12 @@ Last update: 20.5.2020.

//Defining pins used by LinkShield
#define LINK_RPIN 0 //Potentiometer pin
#define LINK_FLEX1_PIN 1 //1st FlexSensor pin
#define LINK_FLEX2_PIN 2 //2nd FlexSensor pin
#define DC_PWM_PIN 5 //DC motor PWM control pin
#define DC_DIR_PIN 6 //DC motor direction pin
#define SERVO_POT_PIN 5
#define LINK_FLEX_PIN 1 //1st FlexSensor pin
#define DC_FWD_PIN 5 //DC motor PWM control pin
#define DC_REV_PIN 6 //DC motor direction pin
#define SERVO_POT_PIN 2

#define ENCODER 0

#define ENCODER_RESOLUTION 1800

#define I2C_CONNECTED 0
Expand Down Expand Up @@ -73,6 +71,8 @@ Last update: 20.5.2020.

class LinkClass { //Creating class
public:

#include "getKalmanEstimate.inl"
void begin(void); //initialization function
void calibrate(); //Calibration function
void actuatorWritePwm(int);
Expand All @@ -90,12 +90,9 @@ class LinkClass { //Creating class

volatile long int count = 0;


float _voltageToPwm;
float _voltageToPwmNew;

#include "getKalmanEstimate.inl"

private:

int _referenceRead;
Expand Down Expand Up @@ -151,8 +148,8 @@ void LinkClass::begin() {
#endif

// define DC controller pins
pinMode(DC_PWM_PIN, OUTPUT);
pinMode(DC_DIR_PIN, OUTPUT);
pinMode(DC_FWD_PIN, OUTPUT);
pinMode(DC_REV_PIN, OUTPUT);

#if ENCODER == 1
// define interrupt pins
Expand Down Expand Up @@ -204,31 +201,30 @@ void LinkClass::calibrate() {

float LinkClass::flexBias(int testLength) {

for (int i = 0; i < testLength; i++ ) { //Make N measurements
_flexSum = _flexSum + analogRead(LINK_FLEX1_PIN);
for (int i = 0; i < testLength; i++ ) { //Make N measurements
_flexSum = _flexSum + flexRead();
}
return _flexSum / testLength; //Compute average
return _flexSum / testLength; //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, -100.00, 100.00);
_referenceValue = AutomationShield.mapFloat((float)_referenceRead, 0.00, 1023.00, -90.00, 90.00)*DEG_TO_RAD;
return _referenceValue;
}

float LinkClass::flexRead() {
_flexRead = analogRead(LINK_FLEX1_PIN);
_flexValue = AutomationShield.mapFloat((float)_flexRead, 500.00, 550.00, 0.00, 2.50)*DEG_TO_RAD;
_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;
}

return -_flexValue;
return _flexValue;
}

#if ENCODER == 1
#if ENCODER
float LinkClass::encoderRead() {
_encoderAngle = float(LinkShield.count) / (ENCODER_RESOLUTION/360)*DEG_TO_RAD;
return _encoderAngle;
Expand All @@ -245,50 +241,48 @@ float LinkClass::servoPotRead() {
void LinkClass::actuatorWritePwm(int _pwmValue) {
if (_pwmValue < 0)
{
digitalWrite(DC_DIR_PIN, LOW);
analogWrite(DC_PWM_PIN, _pwmValue);
digitalWrite(DC_REV_PIN, LOW);
analogWrite(DC_FWD_PIN, _pwmValue);
}
else
{
digitalWrite(DC_DIR_PIN, HIGH);
analogWrite(DC_PWM_PIN, _pwmValue);
digitalWrite(DC_REV_PIN, HIGH);
analogWrite(DC_FWD_PIN, _pwmValue);
}
}

void LinkClass::actuatorWritePercent(float _percentValue) { //Turn DC to desired angle
if (_percentValue < 0)
{
digitalWrite(DC_DIR_PIN, LOW);
analogWrite(DC_PWM_PIN, AutomationShield.percToPwm(-_percentValue));
digitalWrite(DC_REV_PIN, LOW);
analogWrite(DC_FWD_PIN, AutomationShield.percToPwm(-_percentValue));
}
else
{
digitalWrite(DC_DIR_PIN, HIGH);
analogWrite(DC_PWM_PIN, AutomationShield.percToPwm(_percentValue));
digitalWrite(DC_REV_PIN, HIGH);
analogWrite(DC_FWD_PIN, AutomationShield.percToPwm(_percentValue));
}
}

void LinkClass::actuatorWriteNew(float _voltageValueNew) {
_voltageToPwmNew = sq(_voltageValueNew) * 255 / sq(5);


if (_voltageValueNew < 0)
if (_voltageValueNew > 0)
{
analogWrite(DC_DIR_PIN, (int)_voltageToPwmNew);
analogWrite(DC_PWM_PIN, 0);
analogWrite(DC_REV_PIN, (int)_voltageToPwmNew);
analogWrite(DC_FWD_PIN, 0);
}
else if(_voltageValueNew > 0)
else if(_voltageValueNew < 0)
{
analogWrite(DC_PWM_PIN, (int)_voltageToPwmNew);
analogWrite(DC_DIR_PIN, 0);
analogWrite(DC_FWD_PIN, (int)_voltageToPwmNew);
analogWrite(DC_REV_PIN, 0);
}
else
{
analogWrite(DC_PWM_PIN, 255);
analogWrite(DC_DIR_PIN, 255);
analogWrite(DC_FWD_PIN, 0);
analogWrite(DC_REV_PIN, 0);
}


}


Expand All @@ -298,13 +292,13 @@ void LinkClass::actuatorWrite(float _voltageValue) {

if (_voltageValue < 0)
{
digitalWrite(DC_DIR_PIN, LOW);
analogWrite(DC_PWM_PIN, (int)_voltageToPwm);
digitalWrite(DC_REV_PIN, LOW);
analogWrite(DC_FWD_PIN, (int)_voltageToPwm);
}
else
{
digitalWrite(DC_DIR_PIN, HIGH);
analogWrite(DC_PWM_PIN, (int)_voltageToPwm);
digitalWrite(DC_REV_PIN, HIGH);
analogWrite(DC_FWD_PIN, (int)_voltageToPwm);
}
}

Expand Down

0 comments on commit d6e36a5

Please sign in to comment.