diff --git a/main/main.ino b/main/main.ino index 0674200..47b10da 100644 --- a/main/main.ino +++ b/main/main.ino @@ -205,10 +205,12 @@ void setup() // set up controller // TODO : move stuff below - float * initialize={0, 0, 0, 0, 0, 0}; - ws.x=Matrix(6, 1, initialize); - ws.y=Matrix(4, 1, initialize); - ws.u=Matrix(2, 1, initialize); + float * initialize_6={0, 0, 0, 0, 0, 0}; + float * initialize_4={0, 0, 0, 0}; + float * initialize_2={0, 0}; + ws.x=Matrix(6, 1, initialize_6); + ws.y=Matrix(4, 1, initialize_4); + ws.u=Matrix(2, 1, initialize_2); ws.uLast[0] = 0; //last commanded tvc x input ws.uLast[1] = 0; //last commanded tvc y input @@ -273,9 +275,9 @@ void loop() //if so, then updates the control constant by multiplying by the current thrust //construct x //construct y - ws.u=sMult(mMult(), -1); //calculate input + ws.u=(K*ws.x).scale(-1); //calculate input ws.x=mAdd(ws.x, sMult(mAdd(mAdd(), ), ws.dt)); - ws.x+=ws.dt*(ws.A*ws.x+ws.B*ws.u+ws.L*(ws.y-ws.C*ws.x)); //calculate next state + ws.x=ws.x+((A*ws.x)+(B*ws.u)+(L*(ws.y-(C*ws.x)))).scale(ws.dt); //calculate next state //process u //send u to tvc } diff --git a/main/matrix.hh b/main/matrix.hh index ac752e6..fd3b69f 100644 --- a/main/matrix.hh +++ b/main/matrix.hh @@ -25,7 +25,7 @@ class Matrix } Matrix operator+(Matrix); Matrix operator-(Matrix); - Matrix scale (float k); + Matrix scale (float); Matrix operator*(Matrix); }; diff --git a/main/mission_constants.hh b/main/mission_constants.hh index 9c3292a..c4b981a 100644 --- a/main/mission_constants.hh +++ b/main/mission_constants.hh @@ -126,10 +126,10 @@ Matrix B=Matrix(6, 2, B_VALUES); 0,0,0,0,0,0}; //controller gain Matrix K=Matrix(2, 6, K_VALUES); - float C_VALUES [24] = {0,0,0,0,0,0, - 0,0,0,0,0,0, - 0,0,0,0,0,0, - 0,0,0,0,0,0}; //sensor matrix + float C_VALUES [24] = {1,0,0,0,0,0, + 0,0,1,0,0,0, + 0,0,0,1,0,0, + 0,0,0,0,0,1}; //sensor matrix Matrix C=Matrix(4, 6, C_VALUES); float L_VALUES [24] = {0, 0, 0, 0,