Hi,
I'm very new to arduino, I thought I'd be able to pick it up quickly as I have a background in C programming. But I guess not. Anyway, I'm having problem with this bit of code:
#include <MatrixMath.h>
float qcurrent[4][1] = { {0.6},{0.8},{0},{0} };
float qcmd[4][1] = { {0} , {0} , {0} , {1} };
float q1c = qcmd[1][1];
float q2c = qcmd[2][1];
float q3c = qcmd[3][1];
float q4c = qcmd[4][1];
float qe[4][1];
float qcm[4][4] = {
{q4c , q3c , -q2c , -q1c},
{-q3c , q4c , q1c , -q2c},
{q2c,-q1c,q4c,-q3c},
{q1c,q2c,q3c,q4c}
};
void setup() {
Serial.begin(9600);
}
void loop() {
Matrix.Multiply((float*)qcm,(float*)qcurrent,4,4,1,(float*)qe);
// Matrix.Print((float*)qcm,4,4,".");
Serial.print(q1c);
Serial.print("\n");
Serial.print(q2c);
Serial.print("\n");
Serial.print(q3c);
Serial.print("\n");
Serial.print(q4c);
Serial.print("\n");
Serial.print("\n");
Serial.print("\n");
delay(3000);
}
The problem seems to occur at these lines:
float q1c = qcmd[1][1];
float q2c = qcmd[2][1];
float q3c = qcmd[3][1];
float q4c = qcmd[4][1];
The program doesn't seem to be properly assigning the values of q1~4c for some reason.
qcurrent , qcmd and qe are 4 X 1 matrices and qe is a 4 X 4 matrix.
Thanks!