Skip to content

Commit

Permalink
continue adding to main loop and making progress on memory allocation…
Browse files Browse the repository at this point in the history 
… bug
  • Loading branch information
LiamSmego committed Mar 22, 2021
1 parent e821600 commit 8e685c5
Show file tree
Hide file tree
Showing 6 changed files with 271 additions and 33 deletions.
95 changes: 81 additions & 14 deletions main/main.ino
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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; i<param.rows*param.columns; i++)
{
Serial.print(param.values[i]);
Serial.print(" ");
}
Serial.println(" ");
}

/* tvc_abs
*
Expand Down Expand Up @@ -59,8 +71,9 @@ void send_tvc(Matrix u, Matrix * last_u, double yaw)
void setup()
{
// set up pins (OUTPUT and LOW are defined in Arduino.h)


pinMode(15, INPUT);
digitalWrite(15, HIGH);

//TODO flash setup
//flash.begin(9600); // begins flash chip at specified baud rate

Expand Down Expand Up @@ -100,10 +113,17 @@ void loop()

bno055_read_euler_hrp(&ws.myEulerData);
ws.yaw=float(ws.myEulerData.h)/16.00*DEG_2_RAD;
ws.theta_0[0]=ws.myEulerData.r;
ws.theta_1[0]=ws.myEulerData.r;
ws.theta_0[1]=ws.myEulerData.p;
ws.theta_1[1]=ws.myEulerData.p;

//Serial.print(float(ws.myEulerData.h/16.00));
//Serial.print(" ");
//Serial.print(float(ws.myEulerData.r/16.00));
//Serial.print(" ");
//Serial.println(float(ws.myEulerData.p/16.00));

ws.theta_0[0]=float(ws.myEulerData.r/16.00);
ws.theta_1[0]=float(ws.myEulerData.r/16.00);
ws.theta_0[1]=float(ws.myEulerData.p/16.00);
ws.theta_1[1]=float(ws.myEulerData.p/16.00);

// check for moding change conditions
switch (ws.mode)
Expand All @@ -112,8 +132,14 @@ void loop()
{
if (change_mode_to_countdown(millis()>ws.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
Expand All @@ -123,32 +149,48 @@ 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;
}
case(FINAL_COUNTDOWN):
{
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;
}
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;
}
Expand All @@ -157,15 +199,15 @@ 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
{
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;
}
Expand All @@ -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;
}
Expand All @@ -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
}
18 changes: 11 additions & 7 deletions main/matrix.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
#include <iostream>
#include "matrix.hh"
#include <string>

using namespace std;

//matrix subtraction helper function
Expand Down Expand Up @@ -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<A.rows; i++)
{
for (int j = 0; j < A.columns; j++)
{
cout << A.select(i+1,j+1) << " ";
result+= A.select(i+1,j+1);
result+= " ";
}
cout << "\n";
result+="\n";
}
return result;
}

void Matrix:: redefine(float* new_values)
void Matrix:: redefine(float* update_values)
{
values=new_values;
values=update_values;
}

/** for testing
Expand All @@ -134,4 +138,4 @@ int main()
return 0;
}
*/
*/
4 changes: 3 additions & 1 deletion main/matrix.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,8 @@

#include <cstdbool>
#include <cstdint>
#include <string>
using namespace std;


class Matrix
Expand Down Expand Up @@ -31,6 +33,6 @@ class Matrix
Matrix operator*(Matrix);
};

void display_matrix(Matrix);
string display_matrix(Matrix);

#endif // __MODING_HH__
Loading

0 comments on commit 8e685c5

Please sign in to comment.