MatrixMath library - element values not passing into 2 element vector properly, reason unknown for now

Projects General Guidance
1

Hi all! My username is/was Southpark. I had been a regular here in the past, but because I didn't see any email from Arduino.cc about imminent deletion of my account unless I log in for some activity, then it would be deleted. Coincidentally, my account got automatically deleted around mid February this year. But that's ok. I have created this new account to use now.

In any case, I wanted to log in to get some recommendations about a particular issue that I had been recently encountering with the Arduino 'MatrixMath' library. I have just been trying it out with the Arduino MEGA2560.

In my code attached (which includes using the MatrixMath library), I have created a two element matrix, which is Xd on line 10, which is initialised with zeros, ie. {0.0, 0.0}.

In the setup() area, on line 116, I just use my own simple function to set new values for the vector Xd, In this case, on line 116, I set Xd to be [5.0 2.0].

Now, within the loop() part of my program, I simply print the element values of this vector Xd.

The issue I have encountered is that the printed value (when I run the program in the Arduino MEGA2560 and watch the serial monitor screen) is [0 2], instead of [5 2].

But interestingly, if I purposely set the values in line 10 to non-zero values, such as {1E-10, 0.0} or even {0.0, 0.0001}, then the printed element values of vector Xd will turn out to be the desired values that I want to see, which is [5 2].

Also, very interestingly as well, I have also noticed that if I purposely comment-out the Matrix.Multiply line on line 200, then the printed element values of vector Xd also 'magically' become the desired values, which is [5 2].

I have included my code below. Can anyone help me with figuring out why my element values set at line 116 are not properly or entirely transferring into the vector Xd (at line 184)?

At the moment, my code - if it is loaded into my MEGA2560, will print Xd as zero and two. And I don't yet know why it isn't printing out the values that I need, which is five and two.

Thanks in advance!!!

#include <MatrixMath.h>
#define N (2)  


mtx_type Ad[N][N] = { {1.0, 0.002} , {0.0, 0.95} };
mtx_type Bd[N] = {0.000048, 0.048};
mtx_type Cd[N] = {1205.6, 0.0};
mtx_type Dd[1][1] = {0};

mtx_type Xd[N] =  {0.0, 0.0};  // ISSUE is --- the printed element values of vector Xd at line 184 is always [ZERO and 2], unless we set one of these initial Xd element values on line 10 to a non-zero value. Eg. if we initialise line 10 Xd[N] with {1E-15, 0} or even {0 , 0.001}, then the printed values of Xd at line 184 become the desired values [5 2] (which is set at line 116)
mtx_type Fd[N] = {80, 1.6};
mtx_type U[1][1] = {0.0};
mtx_type Up[1][1] = {0.0}; 
mtx_type Ud[1][1] = {0.0}; 
mtx_type L[N] = {0.00016, 0.0077};
mtx_type Yd[1][1] = {0.0};
mtx_type Ya[1][1] = {0.0}; 
mtx_type ERROR[1][1] = {0.0}; 
mtx_type F_out[1][1] = {0.0}; 

mtx_type Ad_out[N] = {0.0, 0.0};
mtx_type Bd_out[N] = {0.0, 0.0};
mtx_type L_out[N] = {0.0, 0.0}; 

mtx_type SETPOINT_RPM1[1][1] = {1000.0}; 
mtx_type SETPOINT_RPM2[1][1] = {2000.0};

float ya;
float yd;


float setpoint_rpm1 = SETPOINT_RPM1[0][0];  
float setpoint_rpm2 = SETPOINT_RPM2[0][0];  

float grad = 0.1;  
float y_int = -100; 
float k = 80; 

mtx_type SETPOINT_PWM1[1][1] = { (SETPOINT_RPM1[0][0] - y_int) / grad }; 
mtx_type SETPOINT_PWM2[1][1] = { (SETPOINT_RPM2[0][0] - y_int) / grad };

mtx_type R[1][1] = { (setpoint_rpm1 - y_int)/grad }; 

mtx_type ACCUM[1][1] = {0.0}; 
float accum = ACCUM[0][0];


int gear_ratio = 21.3;   


float setpoint_rpm = setpoint_rpm1;
int setpoint_pwm = 0;

int pwm_offset = 0;    

float error_rpm = 0;
float accum_error_rpm = 0;
int error_pwm = 0;
float error_rpm_prev = 0;

float u;
float ud;
float up; 
float int_ku; 


float Ad_o;


int ppr = 1024; 
float RPM_raw = 0;
float RPM_array[700] = {};
int index = 0;

float RPM_initial = 0;

int flag = 0; //flag


unsigned long stab_time = 5000000;   

unsigned long duration = 1000000;   
unsigned long observe_ref = 0;


//************************************

unsigned long count_period = 2000;  
unsigned long T = count_period;     
byte number_of_sensors = 1;
unsigned long final_counts;
unsigned long start_time = micros();
unsigned long measured_time;
unsigned long current_time = micros(); 

//*************************************



void setup() {
  // put your setup code here, to run once:

  Serial.begin(115200); 
  delay(100);
  
  TCCR5A = 0; 
  TCCR5B = 0;
  TCNT5 = 0;  
  pinMode( 47, INPUT_PULLUP); 
  
  TCCR5B =  bit (CS50) | bit (CS51) | bit (CS52);

  setpoint_pwm = 4000;

  Serial.println("SETUP");
  pop_Xd(5.0,2.0);    //this function sets element values for vector Xd
  Matrix.Print((mtx_type*)Xd,N,1,"Test 1 Xd"); 
  

  pop_Ya(RPM_raw);
  Matrix.Multiply((mtx_type*)Cd, (mtx_type*)Xd, N, 1, 1, (mtx_type*)Yd);
  Matrix.Subtract((mtx_type*)Ya, (mtx_type*) Yd, 1, 1, (mtx_type*) ERROR);


  Matrix.Print((mtx_type*)Xd,N,1,"Xd");  


  Matrix.Multiply((mtx_type*)Fd, (mtx_type*)Xd, N, 1, 1, (mtx_type*)F_out); 
  Matrix.Subtract((mtx_type*)R, (mtx_type*)F_out, 1, 1, (mtx_type*)U);
  Matrix.Print((mtx_type*)Fd,1,N,"Fd");  

//***************************************************************


  DDRB = DDRB | 0x20; 
  ICR1 = 16383; 
  TCCR1A = 0;
  TCCR1B = 0;
  TCNT1  = 0;
  OCR1A = 0; 
  TCCR1A |= 0x82;   
  TCCR1B |= 0x19;   
  

//************************************************************************

  delay(1000);  
  OCR1A = 0;    



  TCNT5 = 0; 
  observe_ref = micros();
  start_time = micros();

}


void loop() {

if (flag == 0)  
{
  // put your main code here, to run repeatedly:
  current_time = micros();
  
  if (current_time - start_time >= T)  
  {
    flag = 3; //REMOVE THIS LATER!!!!
    TCCR5B = 0; 
    final_counts = TCNT5;
    
    measured_time = current_time - start_time;
    
    RPM_raw = ( (final_counts / float(ppr)) / (count_period*1e-6) ) * 60;
    ya = RPM_raw;
    pop_Ya(ya);


    Serial.println();
    Serial.println();
    Serial.println("LOOP");
    Matrix.Print((mtx_type*)Fd,1,N,"test Fd"); 
   
    Matrix.Print((mtx_type*)Xd,N,1,"Test 2 Xd");
    

    Matrix.Multiply((mtx_type*)Fd, (mtx_type*)Xd, N, 1, 1, (mtx_type*)F_out); 
    Matrix.Multiply((mtx_type*)Cd, (mtx_type*)Xd, N, 1, 1, (mtx_type*)Yd);

    

    u = R[0][0] - F_out[0][0];
    ud = u;
    pop_U(u);
    pop_Ud(ud);

    
    Matrix.Print((mtx_type*)Bd, N, 1, "Bd"); 
    Matrix.Print((mtx_type*)Ud,1,1,"Ud");
    Matrix.Multiply((mtx_type*)Bd, (mtx_type*)Ud, N, 1, 1, (mtx_type*)Bd_out); //when this line 200 is INCLUDED, the elements of vector Xd at line 184 become [ZERO 2] for some reason, instead of being [5 2]
    Matrix.Print((mtx_type*)Bd_out, N, 1, "Bd_out"); 



    accum = 100;
 

      if ( round(accum) > 16383 ) {
        accum  = 16383;
        OCR1A = 16383;
        Serial.println("accumulated error exceeded limit --- auto-limiting");
      }
      else if ( round(accum) <= 16383 ) {
        OCR1A = round(accum); 
      }
      else
      {
        Serial.println("exception");
      }



    current_time = micros();   
    start_time = micros();     

    TCNT5 = 0;  
    TCCR5B =  bit (CS50) | bit (CS51) | bit (CS52); //restart external clock source
  }
 } //flag




} //loop




double pop_Xd (float a, float b) 
{
Xd[0] = a;
Xd[1] = b;
}


void pop_Ya (float a)
{
  Ya[0][0] = a;
}

void pop_Up (float a)
{
  Up[0][0] = a;
}

void pop_U (float a)
{
  U[0][0] = a;
}

void pop_Ud (float a)
{
  Ud[0][0] = a;
}
2

This function has no return statement. Add a return statement or set to void.

Why is Fd a 2,1 vector anf Xd a 1,2 vector?

Where did you find this matrixmath library?
My guess is that it has a constructor to populate a matrix... or maybe an init method...

3

build_1971 --- thanks for mentioning that! I overlooked it. I initially this function set to void, and I had only changed it to 'double' while trying out various avenues to figure why the vector Xd element values are [0 2] in the serial monitor window when I run the program. Unless I do those interesting small changes that I mentioned in my original post, the printed values for vector Xd is [0 2], instead of [5 2].

I will put my code here too - but this time with 'void' for the function pop_Xd. Thanks for letting me know about the 'double'.

The vector Xd is still printing [0 2] with this code below. Almost identical code, just that I have now changed 'double' back to 'void' on line 239.


#include <MatrixMath.h>
#define N (2)  


mtx_type Ad[N][N] = { {1.0, 0.002} , {0.0, 0.95} };
mtx_type Bd[N] = {0.000048, 0.048};
mtx_type Cd[N] = {1205.6, 0.0};
mtx_type Dd[1][1] = {0};

mtx_type Xd[N] =  {0.0, 0.0};  // ISSUE is --- the printed element values of vector Xd at line 184 is always [ZERO and 2], unless we set one of these initial Xd element values on line 10 to a non-zero value. Eg. if we initialise line 10 Xd[N] with {1E-15, 0} or even {0 , 0.001}, then the printed values of Xd at line 184 become the desired values [5 2] (which is set at line 116)
mtx_type Fd[N] = {80, 1.6};
mtx_type U[1][1] = {0.0};
mtx_type Up[1][1] = {0.0}; 
mtx_type Ud[1][1] = {0.0}; 
mtx_type L[N] = {0.00016, 0.0077};
mtx_type Yd[1][1] = {0.0};
mtx_type Ya[1][1] = {0.0}; 
mtx_type ERROR[1][1] = {0.0}; 
mtx_type F_out[1][1] = {0.0}; 

mtx_type Ad_out[N] = {0.0, 0.0};
mtx_type Bd_out[N] = {0.0, 0.0};
mtx_type L_out[N] = {0.0, 0.0}; 

mtx_type SETPOINT_RPM1[1][1] = {1000.0}; 
mtx_type SETPOINT_RPM2[1][1] = {2000.0};

float ya;
float yd;


float setpoint_rpm1 = SETPOINT_RPM1[0][0];  
float setpoint_rpm2 = SETPOINT_RPM2[0][0];  

float grad = 0.1;  
float y_int = -100; 
float k = 80; 

mtx_type SETPOINT_PWM1[1][1] = { (SETPOINT_RPM1[0][0] - y_int) / grad }; 
mtx_type SETPOINT_PWM2[1][1] = { (SETPOINT_RPM2[0][0] - y_int) / grad };

mtx_type R[1][1] = { (setpoint_rpm1 - y_int)/grad }; 

mtx_type ACCUM[1][1] = {0.0}; 
float accum = ACCUM[0][0];


int gear_ratio = 21.3;   


float setpoint_rpm = setpoint_rpm1;
int setpoint_pwm = 0;

int pwm_offset = 0;    

float error_rpm = 0;
float accum_error_rpm = 0;
int error_pwm = 0;
float error_rpm_prev = 0;

float u;
float ud;
float up; 
float int_ku; 


float Ad_o;


int ppr = 1024; 
float RPM_raw = 0;
float RPM_array[700] = {};
int index = 0;

float RPM_initial = 0;

int flag = 0; //flag


unsigned long stab_time = 5000000;   

unsigned long duration = 1000000;   
unsigned long observe_ref = 0;


//************************************

unsigned long count_period = 2000;  
unsigned long T = count_period;     
byte number_of_sensors = 1;
unsigned long final_counts;
unsigned long start_time = micros();
unsigned long measured_time;
unsigned long current_time = micros(); 

//*************************************



void setup() {
  // put your setup code here, to run once:

  Serial.begin(115200); 
  delay(100);
  
  TCCR5A = 0; 
  TCCR5B = 0;
  TCNT5 = 0;  
  pinMode( 47, INPUT_PULLUP); 
  
  TCCR5B =  bit (CS50) | bit (CS51) | bit (CS52);

  setpoint_pwm = 4000;

  Serial.println("SETUP");
  pop_Xd(5.0,2.0);    //this function sets element values for vector Xd
  Matrix.Print((mtx_type*)Xd,N,1,"Test 1 Xd"); 
  

  pop_Ya(RPM_raw);
  Matrix.Multiply((mtx_type*)Cd, (mtx_type*)Xd, N, 1, 1, (mtx_type*)Yd);
  Matrix.Subtract((mtx_type*)Ya, (mtx_type*) Yd, 1, 1, (mtx_type*) ERROR);


  Matrix.Print((mtx_type*)Xd,N,1,"Xd");  


  Matrix.Multiply((mtx_type*)Fd, (mtx_type*)Xd, N, 1, 1, (mtx_type*)F_out); 
  Matrix.Subtract((mtx_type*)R, (mtx_type*)F_out, 1, 1, (mtx_type*)U);
  Matrix.Print((mtx_type*)Fd,1,N,"Fd");  

//***************************************************************


  DDRB = DDRB | 0x20; 
  ICR1 = 16383; 
  TCCR1A = 0;
  TCCR1B = 0;
  TCNT1  = 0;
  OCR1A = 0; 
  TCCR1A |= 0x82;   
  TCCR1B |= 0x19;   
  

//************************************************************************

  delay(1000);  
  OCR1A = 0;    



  TCNT5 = 0; 
  observe_ref = micros();
  start_time = micros();

}


void loop() {

if (flag == 0)  
{
  // put your main code here, to run repeatedly:
  current_time = micros();
  
  if (current_time - start_time >= T)  
  {
    flag = 3; //REMOVE THIS LATER!!!!
    TCCR5B = 0; 
    final_counts = TCNT5;
    
    measured_time = current_time - start_time;
    
    RPM_raw = ( (final_counts / float(ppr)) / (count_period*1e-6) ) * 60;
    ya = RPM_raw;
    pop_Ya(ya);


    Serial.println();
    Serial.println();
    Serial.println("LOOP");
    Matrix.Print((mtx_type*)Fd,1,N,"test Fd"); 
   
    Matrix.Print((mtx_type*)Xd,N,1,"Test 2 Xd");
    

    Matrix.Multiply((mtx_type*)Fd, (mtx_type*)Xd, N, 1, 1, (mtx_type*)F_out); 
    Matrix.Multiply((mtx_type*)Cd, (mtx_type*)Xd, N, 1, 1, (mtx_type*)Yd);

    

    u = R[0][0] - F_out[0][0];
    ud = u;
    pop_U(u);
    pop_Ud(ud);

    
    Matrix.Print((mtx_type*)Bd, N, 1, "Bd"); 
    Matrix.Print((mtx_type*)Ud,1,1,"Ud");
    Matrix.Multiply((mtx_type*)Bd, (mtx_type*)Ud, N, 1, 1, (mtx_type*)Bd_out); //when this line 200 is INCLUDED, the elements of vector Xd at line 184 become [ZERO 2] for some reason, instead of being [5 2]
    Matrix.Print((mtx_type*)Bd_out, N, 1, "Bd_out"); 



    accum = 100;
 

      if ( round(accum) > 16383 ) {
        accum  = 16383;
        OCR1A = 16383;
        Serial.println("accumulated error exceeded limit --- auto-limiting");
      }
      else if ( round(accum) <= 16383 ) {
        OCR1A = round(accum); 
      }
      else
      {
        Serial.println("exception");
      }



    current_time = micros();   
    start_time = micros();     

    TCNT5 = 0;  
    TCCR5B =  bit (CS50) | bit (CS51) | bit (CS52); //restart external clock source
  }
 } //flag




} //loop




double pop_Xd (float a, float b) 
{
Xd[0] = a;
Xd[1] = b;
}


void pop_Ya (float a)
{
  Ya[0][0] = a;
}

void pop_Up (float a)
{
  Up[0][0] = a;
}

void pop_U (float a)
{
  U[0][0] = a;
}

void pop_Ud (float a)
{
  Ud[0][0] = a;
}
4

I knew this would not fix your problem. But it indicates your compiler is not set to 'treat all warnings as errors'. You should set your compiler to be more strict... google the forum for a how to...

5

I am not familiar with this library. But why do you need this typecast?
Why does the matrix library not accept an instance as argument?

1 Like
6

Actually the function pop_Xd() being defined as returning double but not actually returning anything could cause undefined behavior in some cases so it could have been the issue. Definitely something to fix.

7

build_1971 - thanks for recommending to set for more strict. I will google for doing that. Much appreciated.

8

J-M-L - thanks for adding that comment. That was my fault there. It was originally void for that pop_Xd() function. It was due to myself trying various things - just for trial and error testing and looking for behaviour patterns - to see if I needed to set any variables etc to double instead of float etc ----- I had forgotten to just set that one back to void. I have set that function back to void again. The Xd printing out as [0 5] in serial monitor has been interesting - in that the issue goes away (but don't why) if I just change line 10 to say :

mtx_type Xd[N] = {0.0, 0.0};

OR if I simply comment-out (remove) line 200, where line 200 is actually after the matrix print instruction at line 184!

That is - if I do either of the above, then Xd will print what I would like it to print, which is [5 2].

9

can you post your new code?

10

Sure can J-M-L. I added the new code in one of my previous posts. I'll just add it again here. I lost my old account - 'Southpark' because the forum software automatically deleted my account from inactivity - but I didn't see any email coming in that warned of it. Otherwise I would have logged in to remain 'active'.

And because this is a new account, the forum software says I can't 'upload' any files. So at the moment, I can only use the post option to embed the code text within this post. Thanks for asking to post my new/update code!

#include <MatrixMath.h>
#define N (2)  


mtx_type Ad[N][N] = { {1.0, 0.002} , {0.0, 0.95} };
mtx_type Bd[N] = {0.000048, 0.048};
mtx_type Cd[N] = {1205.6, 0.0};
mtx_type Dd[1][1] = {0};

mtx_type Xd[N] =  {0.0, 0.0};  // ISSUE is --- the printed element values of vector Xd at line 184 is always [ZERO and 2], unless we set one of these initial Xd element values on line 10 to a non-zero value. Eg. if we initialise line 10 Xd[N] with {1E-15, 0} or even {0 , 0.001}, then the printed values of Xd at line 184 become the desired values [5 2] (which is set at line 116)
mtx_type Fd[N] = {80, 1.6};
mtx_type U[1][1] = {0.0};
mtx_type Up[1][1] = {0.0}; 
mtx_type Ud[1][1] = {0.0}; 
mtx_type L[N] = {0.00016, 0.0077};
mtx_type Yd[1][1] = {0.0};
mtx_type Ya[1][1] = {0.0}; 
mtx_type ERROR[1][1] = {0.0}; 
mtx_type F_out[1][1] = {0.0}; 

mtx_type Ad_out[N] = {0.0, 0.0};
mtx_type Bd_out[N] = {0.0, 0.0};
mtx_type L_out[N] = {0.0, 0.0}; 

mtx_type SETPOINT_RPM1[1][1] = {1000.0}; 
mtx_type SETPOINT_RPM2[1][1] = {2000.0};

float ya;
float yd;


float setpoint_rpm1 = SETPOINT_RPM1[0][0];  
float setpoint_rpm2 = SETPOINT_RPM2[0][0];  

float grad = 0.1;  
float y_int = -100; 
float k = 80; 

mtx_type SETPOINT_PWM1[1][1] = { (SETPOINT_RPM1[0][0] - y_int) / grad }; 
mtx_type SETPOINT_PWM2[1][1] = { (SETPOINT_RPM2[0][0] - y_int) / grad };

mtx_type R[1][1] = { (setpoint_rpm1 - y_int)/grad }; 

mtx_type ACCUM[1][1] = {0.0}; 
float accum = ACCUM[0][0];


int gear_ratio = 21.3;   


float setpoint_rpm = setpoint_rpm1;
int setpoint_pwm = 0;

int pwm_offset = 0;    

float error_rpm = 0;
float accum_error_rpm = 0;
int error_pwm = 0;
float error_rpm_prev = 0;

float u;
float ud;
float up; 
float int_ku; 


float Ad_o;


int ppr = 1024; 
float RPM_raw = 0;
float RPM_array[700] = {};
int index = 0;

float RPM_initial = 0;

int flag = 0; //flag


unsigned long stab_time = 5000000;   

unsigned long duration = 1000000;   
unsigned long observe_ref = 0;


//************************************

unsigned long count_period = 2000;  
unsigned long T = count_period;     
byte number_of_sensors = 1;
unsigned long final_counts;
unsigned long start_time = micros();
unsigned long measured_time;
unsigned long current_time = micros(); 

//*************************************



void setup() {
  // put your setup code here, to run once:

  Serial.begin(115200); 
  delay(100);
  
  TCCR5A = 0; 
  TCCR5B = 0;
  TCNT5 = 0;  
  pinMode( 47, INPUT_PULLUP); 
  
  TCCR5B =  bit (CS50) | bit (CS51) | bit (CS52);

  setpoint_pwm = 4000;

  Serial.println("SETUP");
  pop_Xd(5.0,2.0);    //this function sets element values for vector Xd
  Matrix.Print((mtx_type*)Xd,N,1,"Test 1 Xd"); 
  

  pop_Ya(RPM_raw);
  Matrix.Multiply((mtx_type*)Cd, (mtx_type*)Xd, N, 1, 1, (mtx_type*)Yd);
  Matrix.Subtract((mtx_type*)Ya, (mtx_type*) Yd, 1, 1, (mtx_type*) ERROR);


  Matrix.Print((mtx_type*)Xd,N,1,"Xd");  


  Matrix.Multiply((mtx_type*)Fd, (mtx_type*)Xd, N, 1, 1, (mtx_type*)F_out); 
  Matrix.Subtract((mtx_type*)R, (mtx_type*)F_out, 1, 1, (mtx_type*)U);
  Matrix.Print((mtx_type*)Fd,1,N,"Fd");  

//***************************************************************


  DDRB = DDRB | 0x20; 
  ICR1 = 16383; 
  TCCR1A = 0;
  TCCR1B = 0;
  TCNT1  = 0;
  OCR1A = 0; 
  TCCR1A |= 0x82;   
  TCCR1B |= 0x19;   
  

//************************************************************************

  delay(1000);  
  OCR1A = 0;    



  TCNT5 = 0; 
  observe_ref = micros();
  start_time = micros();

}


void loop() {

if (flag == 0)  
{
  // put your main code here, to run repeatedly:
  current_time = micros();
  
  if (current_time - start_time >= T)  
  {
    flag = 3; //REMOVE THIS LATER!!!!
    TCCR5B = 0; 
    final_counts = TCNT5;
    
    measured_time = current_time - start_time;
    
    RPM_raw = ( (final_counts / float(ppr)) / (count_period*1e-6) ) * 60;
    ya = RPM_raw;
    pop_Ya(ya);


    Serial.println();
    Serial.println();
    Serial.println("LOOP");
    Matrix.Print((mtx_type*)Fd,1,N,"test Fd"); 
   
    Matrix.Print((mtx_type*)Xd,N,1,"Test 2 Xd");
    

    Matrix.Multiply((mtx_type*)Fd, (mtx_type*)Xd, N, 1, 1, (mtx_type*)F_out); 
    Matrix.Multiply((mtx_type*)Cd, (mtx_type*)Xd, N, 1, 1, (mtx_type*)Yd);

    

    u = R[0][0] - F_out[0][0];
    ud = u;
    pop_U(u);
    pop_Ud(ud);

    
    Matrix.Print((mtx_type*)Bd, N, 1, "Bd"); 
    Matrix.Print((mtx_type*)Ud,1,1,"Ud");
    Matrix.Multiply((mtx_type*)Bd, (mtx_type*)Ud, N, 1, 1, (mtx_type*)Bd_out); //when this line 200 is INCLUDED, the elements of vector Xd at line 184 become [ZERO 2] for some reason, instead of being [5 2]
    Matrix.Print((mtx_type*)Bd_out, N, 1, "Bd_out"); 



    accum = 100;
 

      if ( round(accum) > 16383 ) {
        accum  = 16383;
        OCR1A = 16383;
        Serial.println("accumulated error exceeded limit --- auto-limiting");
      }
      else if ( round(accum) <= 16383 ) {
        OCR1A = round(accum); 
      }
      else
      {
        Serial.println("exception");
      }



    current_time = micros();   
    start_time = micros();     

    TCNT5 = 0;  
    TCCR5B =  bit (CS50) | bit (CS51) | bit (CS52); //restart external clock source
  }
 } //flag




} //loop




void pop_Xd (float a, float b) 
{
Xd[0] = a;
Xd[1] = b;
}


void pop_Ya (float a)
{
  Ya[0][0] = a;
}

void pop_Up (float a)
{
  Up[0][0] = a;
}

void pop_U (float a)
{
  U[0][0] = a;
}

void pop_Ud (float a)
{
  Ud[0][0] = a;
}
11

I just followed some examples from this link here --- MatrixMath example

12

so what' s the issue ? if I run this code I see

SETUP

Test 1 Xd
5.00	
2.00	

Xd
5.00	
2.00	

Fd
80.00	1.60	


LOOP

test Fd
80.00	1.60	

Test 2 Xd
0.00	
2.00	

Bd
0.00	
0.05	

Ud
11000.00	

Bd_out
0.53	
528.00

is that the

in the loop ?

13

when you do

You ask to perform Yd = Cd . Xd and you are telling the Matrix library that Cd is of dimension (N, 1), Xd is of dimension (1, 1) and this Yd would have to be of dimension (N, 1)

Now if you. look at the definitions

➜ correct it's N,1

wrong it's not 1,1

wrong it's not N,1

same happens here

➜ there is something fishy , you probably overwrite the memory right there.

14

J-M-L ... thanks for asking! When we run the code, I would like to see

Test 2 Xd
5.00	
2.00

But the loop portion is currently printing 0.0 and 2.0

Line 116 is where I set the [5 2]. But the loop is currently printing 0 and 2.

But - if I go to line 10, and change the {0.0, 0.0} to something like {0.0001, 0.0} or even {1E-15, 0.0}, then the loop printing will show what I'm wanting, which is [5 2]. Eg. at line 10, if I use this code --

mtx_type Xd[N] = {0.0000001, 0.0};

then the loop will print [5.0 2.0] for Xd, which is what I'm wanting it to do. But at the moment, the loop is giving [0 2], and I don't know what that is due to.

Alternatively, I also get the wanted [5 2] for Xd if I comment-out (omit) line 200, which is this line ...

Matrix.Multiply((mtx_type*)Bd, (mtx_type*)Ud, N, 1, 1, (mtx_type*)Bd_out);

15

I do not think this question was answered yet.
From the initialization it is unclear that they are of different shape.

16

Thanks very much for pointing that out J-M-L. I have gone through the code to amend the rows and columns format, and the Xd value in the loop is printing out what I wanted.

I'll add my updated code - I have now accumulated enough posts to add attachments now.

matrix_math_arduino_MEGA2560_3.ino (5.3 KB)

I definitely messed-up with the matrix row and column number settings. I have just a moment gone into the MatrixMath.cpp file at MatrixMath.cpp file and copied the matrix multiplying dimensions format details from there to my arduino project file - which I have attached now.

[update - adding the embedded text code too below]

#include <MatrixMath.h>
#define N (2)  


mtx_type Ad[N][N] = { {1.0, 0.002} , {0.0, 0.95} };
mtx_type Bd[N] = {0.000048, 0.048};
mtx_type Cd[N] = {1205.6, 0.0};
mtx_type Dd[1][1] = {0};

mtx_type Xd[N] =  {0.0, 0.0};  
mtx_type Fd[N] = {80, 1.6};
mtx_type U[1][1] = {0.0};
mtx_type Up[1][1] = {0.0}; 
mtx_type Ud[1][1] = {0.0}; 
mtx_type L[N] = {0.00016, 0.0077};
mtx_type Yd[1][1] = {0.0};
mtx_type Ya[1][1] = {0.0}; 
mtx_type ERROR[1][1] = {0.0}; 
mtx_type F_out[1][1] = {0.0}; 

mtx_type Ad_out[N] = {0.0, 0.0};
mtx_type Bd_out[N] = {0.0, 0.0};
mtx_type L_out[N] = {0.0, 0.0}; 

mtx_type SETPOINT_RPM1[1][1] = {1000.0}; 
mtx_type SETPOINT_RPM2[1][1] = {2000.0};

float ya;
float yd;


float setpoint_rpm1 = SETPOINT_RPM1[0][0];  
float setpoint_rpm2 = SETPOINT_RPM2[0][0];  

float grad = 0.1;  
float y_int = -100; 
float k = 80; 

mtx_type SETPOINT_PWM1[1][1] = { (SETPOINT_RPM1[0][0] - y_int) / grad }; 
mtx_type SETPOINT_PWM2[1][1] = { (SETPOINT_RPM2[0][0] - y_int) / grad };

mtx_type R[1][1] = { (setpoint_rpm1 - y_int)/grad }; 

mtx_type ACCUM[1][1] = {0.0}; 
float accum = ACCUM[0][0];


int gear_ratio = 21.3;   


float setpoint_rpm = setpoint_rpm1;
int setpoint_pwm = 0;

int pwm_offset = 0;    

float error_rpm = 0;
float accum_error_rpm = 0;
int error_pwm = 0;
float error_rpm_prev = 0;

float u;
float ud;
float up; 
float int_ku; 


float Ad_o;


int ppr = 1024; 
float RPM_raw = 0;
float RPM_array[700] = {};
int index = 0;

float RPM_initial = 0;

int flag = 0; //flag


unsigned long stab_time = 5000000;   

unsigned long duration = 1000000;   
unsigned long observe_ref = 0;


//************************************

unsigned long count_period = 2000;  
unsigned long T = count_period;     
byte number_of_sensors = 1;
unsigned long final_counts;
unsigned long start_time = micros();
unsigned long measured_time;
unsigned long current_time = micros(); 

//*************************************



void setup() {
  // put your setup code here, to run once:

  Serial.begin(115200); 
  delay(100);
  
  TCCR5A = 0; 
  TCCR5B = 0;
  TCNT5 = 0;  
  pinMode( 47, INPUT_PULLUP); 
  
  TCCR5B =  bit (CS50) | bit (CS51) | bit (CS52);

  setpoint_pwm = 4000;

  Serial.println("SETUP");
  pop_Xd(5.0,2.0);    //this function sets element values for vector Xd
  Matrix.Print((mtx_type*)Xd,N,1,"Test 1 Xd"); 
  

  pop_Ya(RPM_raw);
  Matrix.Multiply((mtx_type*)Cd, (mtx_type*)Xd, 1, N, 1, (mtx_type*)Yd);
  Matrix.Subtract((mtx_type*)Ya, (mtx_type*) Yd, 1, 1, (mtx_type*) ERROR);


  Matrix.Print((mtx_type*)Xd,N,1,"Xd");  


  Matrix.Multiply((mtx_type*)Fd, (mtx_type*)Xd, 1, N, 1, (mtx_type*)F_out); 
  Matrix.Subtract((mtx_type*)R, (mtx_type*)F_out, 1, 1, (mtx_type*)U);
  Matrix.Print((mtx_type*)Fd,1,N,"Fd");  

//***************************************************************


  DDRB = DDRB | 0x20; 
  ICR1 = 16383; 
  TCCR1A = 0;
  TCCR1B = 0;
  TCNT1  = 0;
  OCR1A = 0; 
  TCCR1A |= 0x82;   
  TCCR1B |= 0x19;   
  

//************************************************************************

  delay(1000);  
  OCR1A = 0;    



  TCNT5 = 0; 
  observe_ref = micros();
  start_time = micros();

}


void loop() {

if (flag == 0)  
{
  // put your main code here, to run repeatedly:
  current_time = micros();
  
  if (current_time - start_time >= T)  
  {
    flag = 3; //REMOVE THIS LATER!!!!
    TCCR5B = 0; 
    final_counts = TCNT5;
    
    measured_time = current_time - start_time;
    
    RPM_raw = ( (final_counts / float(ppr)) / (count_period*1e-6) ) * 60;
    ya = RPM_raw;
    pop_Ya(ya);


    Serial.println();
    Serial.println();
    Serial.println("LOOP");
    Matrix.Print((mtx_type*)Fd,1,N,"test Fd"); 
   
    Matrix.Print((mtx_type*)Xd,N,1,"Test 2 Xd");
    

    Matrix.Multiply((mtx_type*)Fd, (mtx_type*)Xd, 1, N, 1, (mtx_type*)F_out); 
    Matrix.Multiply((mtx_type*)Cd, (mtx_type*)Xd, 1, N, 1, (mtx_type*)Yd);

    

    u = R[0][0] - F_out[0][0];
    ud = u;
    pop_U(u);
    pop_Ud(ud);

    
    Matrix.Print((mtx_type*)Bd, N, 1, "Bd"); 
    Matrix.Print((mtx_type*)Ud,1,1,"Ud");
    Matrix.Multiply((mtx_type*)Bd, (mtx_type*)Ud, N, 1, 1, (mtx_type*)Bd_out); 


    accum = 100;
 

      if ( round(accum) > 16383 ) {
        accum  = 16383;
        OCR1A = 16383;
        Serial.println("accumulated error exceeded limit --- auto-limiting");
      }
      else if ( round(accum) <= 16383 ) {
        OCR1A = round(accum); 
      }
      else
      {
        Serial.println("exception");
      }



    current_time = micros();   
    start_time = micros();     

    TCNT5 = 0;  
    TCCR5B =  bit (CS50) | bit (CS51) | bit (CS52); //restart external clock source
  }
 } //flag




} //loop




void pop_Xd (float a, float b) 
{
Xd[0] = a;
Xd[1] = b;
}


void pop_Ya (float a)
{
  Ya[0][0] = a;
}

void pop_Up (float a)
{
  Up[0][0] = a;
}

void pop_U (float a)
{
  U[0][0] = a;
}

void pop_Ud (float a)
{
  Ud[0][0] = a;
}


  //Matrix.Multiply formatting
  // A = input matrix (m x p)
	// B = input matrix (p x n)
	// m = number of rows in A
	// p = number of columns in A = number of rows in B
	// n = number of columns in B
	// C = output matrix = A*B (m x n)

Thanks very much - genuinely and seriously to you and the members here that have the time and effort to help me here. I needed it, and also a very good reminder for myself to get the format correct for myself to start with. As in check and keep checking. Thanks JML and all. Have a good weekend ahead - and best regards. Great to be able to communicate with you again too!!!!!

17

True. My Fd is a 1 row by 2 column vector. And my Xd is a 2 row by 1 column vector. And my Cd is a 1 row by 2 column vector, like Fd. Bd has same dimensions at Xd. And Yd is a 1x1, and U and Ud are 1x1 as well. Thanks build_1971 and J-M-L.

18

Glad it was solved!

Have fun

Side note

Short enough code is best posted directly within code tags, I can’t read easily the .imo attachment from my mobile

19

Thanks for mentioning that!! I will go back to the previous post and embed the code there too. Much appreciated again!

20

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.