Skip to content

Commit

Permalink
cleanup
Browse files Browse the repository at this point in the history
  • Loading branch information
LiamSmego committed Mar 26, 2021
1 parent f699bb5 commit dc71f4c
Show file tree
Hide file tree
Showing 2 changed files with 108 additions and 41 deletions.
133 changes: 98 additions & 35 deletions main/main.ino
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,52 +32,44 @@ void main_display_matrix(Matrix param, float scale)
*
* TODO: add docstring
*/
Matrix send_tvc(Matrix u, Matrix last_u, float yaw)
Matrix send_tvc(Matrix u, Matrix last_u, float yaw, Servo x, Servo y)
{
//scale down angle to physical tvc limit
float input_magnitude=sqrtf(u.values[0]*u.values[0]+u.values[1]*u.values[1]);
if (input_magnitude>MAX_U)
{
u.scale(MAX_U/input_magnitude);
}
//main_display_matrix(u, RAD_2_DEG);

if(NULL_U_X_AXIS){ u.values[0]=0; }
if(NULL_U_Y_AXIS){ u.values[1]=0; }
//main_display_matrix(u, RAD_2_DEG);

vector<float> rotation_values {cos(yaw), -sin(yaw), sin(yaw), cos(yaw)};
vector<float> rotation_values {cos(yaw+BETA), -sin(yaw+BETA), sin(yaw+BETA), cos(yaw+BETA)};
Matrix R=Matrix(2, 2, rotation_values);
u=R*u;
//main_display_matrix(u, RAD_2_DEG);


Matrix travel=u-last_u;
float travel_magnitude=sqrtf(travel.values[0]*travel.values[0]+travel.values[1]*travel.values[1]);
/*
if (travel_magnitude<MIN_STEP)
u.values[0]=GEAR*RAD_2_DEG*u.values[0]+TVC_X_OFFSET;
u.values[1]=GEAR*RAD_2_DEG*u.values[1]+TVC_Y_OFFSET;
float delta=u.values[0]-last_u.values[0];
if (abs(delta)>MAX_U_DELTA)
{
u=last_u;
u.values[0]=last_u.values[0]+delta/abs(delta)*MAX_U_DELTA;
}
else if (travel_magnitude>SERVO_SPEED*ws.dt)
delta=u.values[1]-last_u.values[1];
if (abs(delta)>MAX_U_DELTA)
{
u=last_u+travel.scale(SERVO_SPEED*ws.dt/travel_magnitude);
u.values[1]=last_u.values[1]+delta/abs(delta)*MAX_U_DELTA;
}
*/
//main_display_matrix(u, RAD_2_DEG);

Serial.print(u.values[0]);
Serial.print(" ");
Serial.println(u.values[1]);



//rotate input are body z axis
rotation_values = {cos(BETA), -sin(BETA), sin(BETA), cos(BETA)};
R.values=rotation_values;
Matrix new_u=R*u;
Serial.println(" ");

//convert to degrees, gear the angle, and round
ws.tvc_x.write(round(GEAR*RAD_2_DEG*new_u.values[0])+TVC_X_OFFSET);
ws.tvc_y.write(round(GEAR*RAD_2_DEG*new_u.values[1])+TVC_Y_OFFSET);
if (TVC_ENABLE)
{

x.write(round(u.values[0]));
y.write(round(u.values[1]));
}
delay(1);

return u;
Expand All @@ -98,13 +90,20 @@ void setup()
digitalWrite(0, LOW);

pinMode(G_LED_PIN, OUTPUT);
<<<<<<< Updated upstream
pinMode(R_LED_PIN, OUTPUT);
pinMode(B_LED_PIN, OUTPUT);

// LED - white, setup
digitalWrite(R_LED_PIN, LOW);
digitalWrite(G_LED_PIN, LOW);
digitalWrite(B_LED_PIN, LOW);
=======
digitalWrite(G_LED_PIN, HIGH);

pinMode(B_LED_PIN, OUTPUT);
digitalWrite(B_LED_PIN, HIGH);
>>>>>>> Stashed changes

//TODO flash setup
//flash.begin(9600); // begins flash chip at specified baud rate
Expand All @@ -113,14 +112,23 @@ void setup()
ws.mode = STARTUP_STABLE;

// set up Thrust-Vector Controller (TVC)
ws.tvc_x.attach(TVC_X_PIN); // attaches declared servo to specified pin
ws.tvc_y.attach(TVC_Y_PIN); // attaches declared servo to specified pin

ws.tvc_x.write(TVC_X_OFFSET);
ws.tvc_y.write(TVC_Y_OFFSET);
if (TVC_ENABLE || TVC_TEST)
{
ws.tvc_x.attach(TVC_X_PIN); // attaches declared servo to specified pin
ws.tvc_y.attach(TVC_Y_PIN); // attaches declared servo to specified pin
ws.tvc_x.write(TVC_X_OFFSET);
ws.tvc_y.write(TVC_Y_OFFSET);
}

//Initialize I2C communication
Wire.begin();
// join I2C bus (I2Cdev library doesn't do this automatically)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif

delay(1000);

//Initialization of the BNO055
BNO_Init(&ws.bno_1); //Assigning the structure to hold information about the device
Expand All @@ -139,6 +147,33 @@ void setup()

delay(1);

if(TVC_TEST)
{
Matrix test_u=Matrix(2, 1, ws.initialize_2);
vector<float> test_us {MAX_U,-MAX_U,MAX_U, -MAX_U, 0};

for (int i=0; i<10; i++)
{
if (i<5)
{
test_u.values[0]=test_us[i];
test_u.values[1]=0;
}
else
{
test_u.values[1]=test_us[i-5];
test_u.values[0]=0;
}

vector<float> test_rotation {cos(BETA), -sin(BETA), sin(BETA), cos(BETA)};
Matrix test_R=Matrix(2, 2, test_rotation);
test_u=test_R*test_u;
ws.tvc_x.write(round(GEAR*RAD_2_DEG*test_u.values[0])+TVC_X_OFFSET);
ws.tvc_y.write(round(GEAR*RAD_2_DEG*test_u.values[1])+TVC_Y_OFFSET);
delay(1000);
}
}

ws.next_mode_time=millis()+STARTUP_STABLE_PERIOD*KILO_I;
}

Expand Down Expand Up @@ -214,6 +249,7 @@ void loop()
digitalWrite(B_LED_PIN, HIGH)
ws.next_mode_time=millis()+PREP_TVC_PERIOD*KILO_I;
ws.mode = PREP_TVC;
digitalWrite(B_LED_PIN, LOW);
}
else
{
Expand All @@ -239,7 +275,7 @@ void loop()
{
ws.construct_y();
ws.u=(KC*ws.x).scale(-1);//calculate input
ws.last_u=send_tvc(ws.u, ws.last_u, ws.yaw);
ws.last_u=send_tvc(ws.u, ws.last_u, ws.yaw, ws.tvc_x, ws.tvc_y);
ws.construct_x(false);
}
break;
Expand Down Expand Up @@ -267,7 +303,7 @@ void loop()
{
ws.construct_y();
ws.u=(KC*ws.x).scale(-1); //calculate input
ws.last_u=send_tvc(ws.u, ws.last_u, ws.yaw);
ws.last_u=send_tvc(ws.u, ws.last_u, ws.yaw, ws.tvc_x, ws.tvc_y);
ws.construct_x(KALMAN_ENABLED);
}
break;
Expand All @@ -280,6 +316,33 @@ void loop()
}

}

bool zero=true;
for (int i=0; i<ws.x.columns*ws.x.rows; i++)
{
if (zero)
{
zero=ws.x.values[i]==0;
}
}
if (zero)
{
digitalWrite(G_LED_PIN, LOW);
}
else
{
digitalWrite(G_LED_PIN, HIGH);
}
/*
if (!(ws.last_u.values[0]==0 && ws.last_u.values[1]==0))
{
digitalWrite(G_LED_PIN, LOW);
}
else
{
digitalWrite(G_LED_PIN, HIGH);
}
*/
Serial.println(ws.mode);
main_display_matrix(ws.x, 1);
//Serial.print(millis());
Expand Down
16 changes: 10 additions & 6 deletions main/mission_constants.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,8 @@ enum Mode {
SHUTDOWN_STABLE = 5
};

const bool TVC_TEST=false;
const bool TVC_ENABLE=true;
const bool KALMAN_ENABLED=false;
const bool NULL_U_X_AXIS=false;
const bool NULL_U_Y_AXIS=false;
Expand Down Expand Up @@ -69,15 +71,17 @@ const int FLASH_CS_PIN = 0;
const int IMU_1_PIN=0;
const int IMU_2_PIN=0;
const int R_LED_PIN=0;
const int B_LED_PIN=0;
const int G_LED_PIN=4;
const int B_LED_PIN=4;
const int G_LED_PIN=6;

// Thrust-Vector Controller (TVC) //TODO: modify values
const float GEAR = 9; // gearing ratio of the servo to the TVC //9
const float SERVO_SPEED=3;
const float GEAR = 7.5; // gearing ratio of the servo to the TVC //9
const float SERVO_SPEED=1.5;
const float MAX_U_DELTA=2;
const float TVC_LOOP=5;
const float MIN_STEP=DEG_2_RAD/GEAR;
const float TVC_X_OFFSET = 100; // TODO: add description
const float TVC_Y_OFFSET = 55; // TODO: add description
const float TVC_X_OFFSET = 107.5; // TODO: add description
const float TVC_Y_OFFSET = 67.5; // TODO: add description
const float MAX_U= 5*DEG_2_RAD; //maximum gimbal angle
const float BETA = 135*DEG_2_RAD; // angle (rad) that corrects for misalignment between body frame and TVC frame

Expand Down

0 comments on commit dc71f4c

Please sign in to comment.