From 8e685c51d6eb0900224db47d4af9d4f469b8cafc Mon Sep 17 00:00:00 2001 From: LiamSmego Date: Sun, 21 Mar 2021 23:13:01 -0400 Subject: [PATCH] continue adding to main loop and making progress on memory allocation bug --- main/main.ino | 95 +++++++++++++++--- main/matrix.cpp | 18 ++-- main/matrix.hh | 4 +- main/matrix_update_reference.txt | 165 +++++++++++++++++++++++++++++++ main/mission_constants.hh | 20 ++-- main/workspace.hh | 2 +- 6 files changed, 271 insertions(+), 33 deletions(-) create mode 100644 main/matrix_update_reference.txt diff --git a/main/main.ino b/main/main.ino index 5a757e5..92e767d 100644 --- a/main/main.ino +++ b/main/main.ino @@ -18,6 +18,18 @@ using namespace std; // (--) instance of workspace class storing all the variables used in the loop Workspace ws; +int count=0; + + +void main_display_matrix(Matrix param) +{ + for (int i=0; iws.next_mode_time)) { + Serial.println(" "); + Serial.print("----------------------- "); + Serial.print(ws.mode); + Serial.print("t"); + Serial.print(" -----------------------"); + Serial.println(" "); transition_to_countdown(); - ws.next_mode_time=millis()+COUNTDOWN*KILO_I; + ws.next_mode_time=millis()+COUNTDOWN_PERIOD*KILO_I; ws.mode = COUNTDOWN; } else @@ -123,17 +149,27 @@ void loop() break; } case (COUNTDOWN): - { + { if (change_mode_to_final_countdown(millis()>ws.next_mode_time)) { + Serial.println(" "); + Serial.print("----------------------- "); + Serial.print(ws.mode); + Serial.print("t"); + Serial.print(" -----------------------"); + Serial.println(" "); + transition_to_final_countdown(); + Serial.println("here"); ws.next_mode_time=millis()+FINAL_COUNTDOWN_PERIOD*KILO_I; + Serial.println("here"); ws.mode = FINAL_COUNTDOWN; + Serial.println("here"); } else { ws.construct_y(); - ws.x=ws.x+(L*(ws.y-(C*ws.x))).scale(ws.dt/MEGA); //state estimation only using sensors + //ws.x=ws.x+(L*(ws.y-(C*ws.x))).scale(ws.dt); //state estimation only using sensors } break; } @@ -141,6 +177,12 @@ void loop() { if (change_mode_to_prep_tvc(millis()>ws.next_mode_time)) { + Serial.println(" "); + Serial.print("----------------------- "); + Serial.print(ws.mode); + Serial.print("t"); + Serial.print(" -----------------------"); + Serial.println(" "); transition_to_prep_tvc(); ws.next_mode_time=millis()+PREP_TVC_PERIOD*KILO_I; ws.mode = PREP_TVC; @@ -148,7 +190,7 @@ void loop() else { ws.construct_y(); - ws.x=ws.x+(L*(ws.y-(C*ws.x))).scale(ws.dt/MEGA); //state estimation only using sensors + //ws.x=ws.x+(L*(ws.y-(C*ws.x))).scale(ws.dt); //state estimation only using sensors } break; } @@ -157,7 +199,7 @@ void loop() if (change_mode_to_burn_baby_burn(millis()>ws.next_mode_time)) { transition_to_burn_baby_burn(); - ws.next_mode_time=millis()+BURN_BABY_BURN*KILO_I; + ws.next_mode_time=millis()+BURN_BABY_BURN_PERIOD*KILO_I; ws.mode = BURN_BABY_BURN; } else @@ -165,7 +207,7 @@ void loop() ws.construct_y(); ws.u=(KC*ws.x).scale(-1);//calculate input send_tvc(ws.u, &ws.last_u, ws.yaw); - ws.x=ws.x+(L*(ws.y-(C*ws.x))).scale(ws.dt/MEGA); //state estimation only using sensors + ws.x=ws.x+(L*(ws.y-(C*ws.x))).scale(ws.dt); //state estimation only using sensors } break; } @@ -181,7 +223,7 @@ void loop() ws.construct_y(); ws.u=(KC*ws.x).scale(-1); //calculate input send_tvc(ws.u, &ws.last_u, ws.yaw); - ws.x=ws.x+(A*ws.x+B*ws.last_u+L*(ws.y-(C*ws.x))).scale(ws.dt/MEGA); //state estimation using Kalman filter + ws.x=ws.x+(A*ws.x+B*ws.last_u+L*(ws.y-(C*ws.x))).scale(ws.dt); //state estimation using Kalman filter } break; } @@ -190,8 +232,33 @@ void loop() // TODO: implement this block break; } - display_matrix(ws.y); + } + + + //count++; + //if(count%150==0) + //{ + //main_display_matrix(ws.y); + //main_display_matrix(ws.x); + Serial.println(millis()); + Serial.println(ws.next_mode_time); + Serial.println(); + //Serial.print(ws.theta_0[0]); + //Serial.print(" "); + //Serial.print(ws.theta_0[1]); + //Serial.print(" "); + //Serial.println(ws.theta_0[2]); + + //Serial.print(ws.theta_1[0]); + //Serial.print(" "); + //Serial.print(ws.theta_1[1]); + //Serial.print(" "); + //Serial.println(ws.theta_1[2]); + //Serial.println(" "); + //} + + //Serial.println(ws.dt); // TODO: add data record // TODO: add (somewhere else) data struct } diff --git a/main/matrix.cpp b/main/matrix.cpp index 1533d27..acd72be 100644 --- a/main/matrix.cpp +++ b/main/matrix.cpp @@ -1,5 +1,6 @@ -#include #include "matrix.hh" +#include + using namespace std; //matrix subtraction helper function @@ -102,21 +103,24 @@ Matrix Matrix::operator-(Matrix B) return Matrix(m_a, n_a, new_values); } -void display_matrix(Matrix A) +string display_matrix(Matrix A) { + std::string result; for (int i=0; i #include +#include +using namespace std; class Matrix @@ -31,6 +33,6 @@ class Matrix Matrix operator*(Matrix); }; -void display_matrix(Matrix); +string display_matrix(Matrix); #endif // __MODING_HH__ diff --git a/main/matrix_update_reference.txt b/main/matrix_update_reference.txt new file mode 100644 index 0000000..eff3cc3 --- /dev/null +++ b/main/matrix_update_reference.txt @@ -0,0 +1,165 @@ +#include +using namespace std; + +class Matrix +{ + public: + int rows; + int columns; + float * values; + + Matrix(int row_number, int column_number, float * mat_values) + { + rows=row_number; + columns=column_number; + values=mat_values; + } + + float select(int row, int column) + { + return values[(row-1)*columns+(column-1)]; + } + + void redefine(float*); + Matrix operator+(Matrix); + Matrix operator-(Matrix); + Matrix scale (float); + Matrix operator*(Matrix); +}; + +//matrix subtraction helper function +float * m_sub_helper(float * A, float * B, int elements) { + float * C; + C = new float[elements]; + for (int i = 0; i < elements; i++) { + C[i] = A[i] - B[i]; + } + delete C; + return C; +} + +//scalar multiplication helper function +float * s_mult_helper(float * A, float rho, int elements) { + float * C; + C = new float[elements]; + for (int i = 0; i < elements; i++) { + C[i] = A[i] * rho; + } + delete C; + return C; +} + +//matrix multiplication helper function +float * m_mult_helper(float * A, float * B, int m, int n, int p) { + float * C; + C=new float[m*p]; + for (int i = 0; i < m; i++) + { + for (int j = 0; j < p; j++) + { + C[i*p + j] = 0; + for (int k = 0; k < n; k++) + { + C[i*p + j] += A[i*n + k] * B[j + p * k]; + } + } + } + delete C; + return C; +} + +//matrix addition helper function +float * m_add_helper(float * A, float * B, int elements) { + float * C; + C = new float[elements]; + for (int i = 0; i < elements; i++) { + C[i] = A[i] + B[i]; + } + delete C; + return C; +} + +//matrix multiplication +Matrix Matrix::operator*(Matrix B) +{ + const int m=rows; + const int p=B.columns; + const int n=columns; + float * new_values; + if (columns==B.rows) + { + new_values=m_mult_helper(values, B.values, m, n, p); + } + return Matrix(m, p, new_values); +} + +//matrix addition function +Matrix Matrix::operator+(Matrix B) +{ + const int m_a=rows; + const int n_b=B.columns; + const int n_a=columns; + const int m_b=B.rows; + float * new_values; + if ((m_a==m_b)&&(n_a==n_b)) + { + new_values=m_add_helper(values, B.values, m_a*n_a); + } + return Matrix(m_a, n_a, new_values); +} + +//scalar multiplication function +Matrix Matrix :: scale(float k) +{ + float * new_values; + new_values=s_mult_helper(values, k, rows*columns); + return Matrix(rows, columns, new_values); +} + +//matrix subtraction function +Matrix Matrix::operator-(Matrix B) +{ + const int m_a=rows; + const int n_b=B.columns; + const int n_a=columns; + const int m_b=B.rows; + float * new_values; + if ((m_a==m_b)&&(n_a==n_b)) + { + new_values=m_sub_helper(values, B.values, m_a*n_a); + } + return Matrix(m_a, n_a, new_values); +} + +void Matrix:: redefine(float* update_values) +{ + values=update_values; +} + +void display_matrix(Matrix A) +{ + for (int i=0; i