diff --git a/examples/LinkShield/LinkShield_Identification/LinkShield_Identification.ino b/examples/LinkShield/LinkShield_Identification/LinkShield_Identification.ino index b31937a3..5eaacf65 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, 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 @@ -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(", "); diff --git a/examples/LinkShield/LinkShield_LQ/LinkShield_LQ.ino b/examples/LinkShield/LinkShield_LQ/LinkShield_LQ.ino index f7b03d11..589429e7 100644 --- a/examples/LinkShield/LinkShield_LQ/LinkShield_LQ.ino +++ b/examples/LinkShield/LinkShield_LQ/LinkShield_LQ.ino @@ -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}; @@ -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 @@ -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(); @@ -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); diff --git a/matlab/examples/LinkShield/LinkShield_Identification_TF.m b/matlab/examples/LinkShield/LinkShield_Identification_TF.m index a1f2880b..0d3d63bf 100644 --- a/matlab/examples/LinkShield/LinkShield_Identification_TF.m +++ b/matlab/examples/LinkShield/LinkShield_Identification_TF.m @@ -22,13 +22,13 @@ 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 @@ -36,9 +36,10 @@ 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 @@ -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 \ No newline at end of file +ylabel('Angle (rad)') % Y axis label \ No newline at end of file diff --git a/matlab/examples/LinkShield/LinkShield_LQ_calculation.m b/matlab/examples/LinkShield/LinkShield_LQ_calculation.m index 6973f8bf..743f66cf 100644 --- a/matlab/examples/LinkShield/LinkShield_LQ_calculation.m +++ b/matlab/examples/LinkShield/LinkShield_LQ_calculation.m @@ -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); diff --git a/matlab/examples/LinkShield/sys.mat b/matlab/examples/LinkShield/sys.mat index ddb32b18..728f312c 100644 Binary files a/matlab/examples/LinkShield/sys.mat and b/matlab/examples/LinkShield/sys.mat differ diff --git a/src/LinkShield.h b/src/LinkShield.h index e75e02c1..2d51846d 100644 --- a/src/LinkShield.h +++ b/src/LinkShield.h @@ -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 @@ -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); @@ -90,12 +90,9 @@ class LinkClass { //Creating class volatile long int count = 0; - float _voltageToPwm; float _voltageToPwmNew; - #include "getKalmanEstimate.inl" - private: int _referenceRead; @@ -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 @@ -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; @@ -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); } - - } @@ -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); } }