Delta TVC Mount R2

A new customizable TVC mount that can stabilize your model rocket!

Sep 29, 2020

•

7015 views

•

5 respects

Components and supplies

Arduino UNO

Teensy 3.6

Tools and machines

Soldering iron (generic)

Apps and platforms

Arduino IDE

Project description

Code

CODE NOT AVALIABLE

c_cpp

1ODE NOT AVALIABL

ODE NOT AVALIABL

OmegaSoft-2.38.ino

c_cpp

1/* Delta Space Systems
2      Version 2.31
3    September, 10th 2020   */
4
5/* Tuning Guide - README
6 *  To tune the servo offsets for your TVC   Mount go to line 52. 
7    To tune just raise the values until the servo switches   directions then home in on the perfect value.
8 *  To change the ratio between   the Servo and the TVC mount go to line 64. 
9    Use a seperate sketch and raise   the degree the servo moves until it hits the end of the TVC Mount. 
10    Then   divide the servo degree by 5.
11 *  For PID Value Changes go to line 112 - 114.
12   *  To change what I/O pins the servos are connected to go to line 139 - 140.  
13   */
14
15
16/*System State: 
17 * 0 = Go/No Go before launch
18 * 1 = PID Controlled   Ascent
19 * 2 = MECO
20 * 3 = Abort
21 */
22
23//Libraries
24#include <SD.h>
25#include   <SPI.h>
26#include <Servo.h>
27#include <Wire.h>
28#include <MPU6050_tockn.h>
29#include   <math.h>
30#include "BMI088.h"
31#include <BMP280_DEV.h>                           
32
33/*   accel object */
34Bmi088Accel accel(Wire,0x18);
35/* gyro object */
36Bmi088Gyro   gyro(Wire,0x68);
37
38float temperature, pressure, altitude;            
39BMP280_DEV   bmp280;     
40
41double PIDX, PIDY, errorX, errorY, previous_errorX, previous_errorY,   pwmX, pwmY, previouslog, OutX, OutY, OutZ, OreX, OreY, OreZ;
42double PreviousGyroX,   PreviousGyroY, PreviousGyroZ, IntGyroX, IntGyroY, RADGyroX, RADGyroY, RADGyroZ,   RawGyZ, DifferenceGyroX, DifferenceGyroY, DifferenceGyroZ, matrix1, matrix2, matrix3;
43double   matrix4, matrix5, matrix6, matrix7, matrix8, matrix9, PrevGyX, PrevGyY, PrevGyZ,   RawGyX, RawGyY, GyroAngleX, GyroAngleY, GyroAngleZ, GyroRawX, GyroRawY, GyroRawZ;
44
45//Upright   Angle of the Flight Computer
46int desired_angleX = 0;//servoY
47int desired_angleY   = 0;//servoX
48
49//Offsets for tuning 
50int servoY_offset = 115;
51int servoX_offset   = 162;
52
53//Position of servos through the startup function
54int servoXstart   = servoY_offset;
55int servoYstart = servoX_offset;
56
57//The amount the servo   moves by in the startup function
58int servo_start_offset = 20;
59
60//Ratio   between servo gear and tvc mount
61float servoX_gear_ratio = 8;
62float servoY_gear_ratio   = 8;
63
64double OrientationX = 0;
65double OrientationY = 0;
66double OrientationZ   = 1;
67double Ax;
68double Ay;
69
70int ledred = 2;    // LED connected to digital   pin 9
71int ledblu = 5;    // LED connected to digital pin 9
72int ledgrn = 6;     // LED connected to digital pin 9
73int pyro1 = 24;
74int buzzer = 21;
75int   teensyled = 13;
76
77Servo servoX;
78Servo servoY;
79
80double dt, currentTime,   previousTime;
81
82//SD CARD CS
83const int chipSelect = BUILTIN_SDCARD;
84
85//"P"   Constants
86float pidX_p = 0;
87float pidY_p = 0;
88
89//"I" Constants
90float   pidY_i = 0;
91float pidX_i = 0;
92
93//"D" Constants
94float pidX_d = 0;
95float   pidY_d = 0;
96
97
98//PID Gains
99double kp = 0.11;
100double ki = 0.04;
101double   kd = 0.025;
102
103int state;
104 
105void setup(){
106 
107  Serial.begin(9600);
108   Wire.begin();
109  servoX.attach(29);
110  servoY.attach(30);
111  bmp280.begin(BMP280_I2C_ALT_ADDR);               
112  bmp280.startNormalConversion();  
113 
114  pinMode(ledblu,   OUTPUT);
115  pinMode(ledgrn, OUTPUT);
116  pinMode(ledred, OUTPUT);
117  pinMode(buzzer,   OUTPUT);
118  pinMode(pyro1, OUTPUT);
119  pinMode(teensyled, OUTPUT);
120
121  startup();
122   sdstart();
123  launchpoll();
124  
125}
126void loop() {
127  //Defining Time   Variables      
128  currentTime = millis();            
129  dt = (currentTime   - previousTime) / 1000; 
130  launchdetect();
131  datadump();
132  previousTime   = currentTime;  
133}
134
135void rotationmatrices () {
136  
137  //Change Variable   so its easier to refrence later on
138  GyroRawX = (gyro.getGyroY_rads());
139  GyroRawY   = (gyro.getGyroZ_rads());
140  GyroRawZ = (gyro.getGyroX_rads());
141
142  //Integrate   over time to get Local Orientation
143  GyroAngleX += GyroRawX * dt;
144  GyroAngleY   += GyroRawY * dt;
145  GyroAngleZ += GyroRawZ * dt;
146
147  PreviousGyroX = RADGyroX;
148   PreviousGyroY = RADGyroY;
149  PreviousGyroZ = RADGyroZ;
150  
151  RADGyroX =   GyroAngleX;
152  RADGyroY = GyroAngleY;
153  RADGyroZ = GyroAngleZ;
154  
155  DifferenceGyroX   = (RADGyroX - PreviousGyroX);
156  DifferenceGyroY = (RADGyroY - PreviousGyroY);
157   DifferenceGyroZ = (RADGyroZ - PreviousGyroZ);
158
159  OreX = OrientationX;
160   OreY = OrientationY;
161  OreZ = OrientationZ;
162  
163 //X Matrices
164  matrix1   = (cos(DifferenceGyroZ) * cos(DifferenceGyroY));
165  matrix2 = (((sin(DifferenceGyroZ)   * -1) * cos(DifferenceGyroX) + (cos(DifferenceGyroZ)) * sin(DifferenceGyroY) * sin(DifferenceGyroX)));
166   matrix3 = ((sin(DifferenceGyroZ) * sin(DifferenceGyroX) + (cos(DifferenceGyroZ))   * sin(DifferenceGyroY) * cos(DifferenceGyroX)));
167  
168 //Y Matrices
169  matrix4   = sin(DifferenceGyroZ) * cos(DifferenceGyroY);
170  matrix5 = ((cos(DifferenceGyroZ)   * cos(DifferenceGyroX) + (sin(DifferenceGyroZ)) * sin(DifferenceGyroY) * sin(DifferenceGyroX)));
171   matrix6 = (((cos(DifferenceGyroZ) * -1) * sin(DifferenceGyroX) + (sin(DifferenceGyroZ))   * sin(DifferenceGyroY) * cos(DifferenceGyroX)));
172
173 //Z Matrices
174  matrix7   = (sin(DifferenceGyroY)) * -1;
175  matrix8 = cos(DifferenceGyroY) * sin(DifferenceGyroX);
176   matrix9 = cos(DifferenceGyroY) * cos(DifferenceGyroX);
177
178 
179 OrientationX   = ((OreX * matrix1)) + ((OreY * matrix2)) + ((OreZ * matrix3));
180 OrientationY   = ((OreX * matrix4)) + ((OreY * matrix5)) + ((OreZ * matrix6));
181 OrientationZ   = ((OreX * matrix7)) + ((OreY * matrix8)) + ((OreZ * matrix9));
182
183Ax = asin(OrientationX)   * (-180 / PI);
184Ay = asin(OrientationY) * (180 / PI);
185pidcompute();
186
187}
188
189void   pidcompute () {
190previous_errorX = errorX;
191previous_errorY = errorY; 
192
193errorX   = Ax - desired_angleX;
194errorY = Ay - desired_angleY;
195
196//Defining "P"   
197pidX_p = kp * errorX;
198pidY_p = kp * errorY;
199
200//Defining "D"
201pidX_d   = kd*((errorX - previous_errorX)/dt);
202pidY_d = kd*((errorY - previous_errorY)/dt);
203
204//Defining   "I"
205pidX_i = ki * (pidX_i + errorX * dt);
206pidY_i = ki * (pidY_i + errorY   * dt);
207
208//Adding it all up
209PIDX = pidX_p + pidX_i + pidX_d;
210PIDY = pidY_p   + pidY_i + pidY_d;
211 
212pwmY = ((PIDY * servoY_gear_ratio) + servoX_offset);
213pwmX   = ((PIDX * servoX_gear_ratio) + servoY_offset); 
214
215 
216//Servo outputs
217servoX.write(pwmX);
218servoY.write(pwmY);
219
220
221}
222
223void   startup () {
224  tone(buzzer, 1050, 800);
225  digitalWrite(ledblu, HIGH);
226   servoX.write(servoXstart);
227  servoY.write(servoYstart);
228  delay(500);
229   digitalWrite(ledblu, LOW);
230  digitalWrite(ledgrn, HIGH);
231  servoX.write(servoXstart   + servo_start_offset);
232  delay(200);
233  servoY.write(servoYstart + servo_start_offset);
234   digitalWrite(teensyled, HIGH);
235  delay(200);
236  digitalWrite(ledgrn, LOW);
237   digitalWrite(ledred, HIGH);
238  servoX.write(servoXstart);
239  delay(200);
240   servoY.write(servoYstart);
241  delay(200);
242  digitalWrite(ledred, LOW);
243   digitalWrite(ledblu, HIGH);
244  tone(buzzer, 1100, 300);
245  servoX.write(servoXstart   - servo_start_offset);
246  delay(200);
247  tone(buzzer, 1150, 250);
248  servoY.write(servoYstart   - servo_start_offset);
249  delay(200);
250  tone(buzzer, 1200, 200);
251  servoX.write(servoXstart);
252   delay(200);
253  servoY.write(servoYstart);
254  delay(500);
255  
256  
257 }
258void   launchdetect () {
259  
260  accel.readSensor();
261  if (state == 0 && accel.getAccelX_mss()   > 13) {
262  state++;
263  }
264  if (state == 1) {
265  gyro.readSensor();
266   digitalWrite(ledred, LOW);
267  digitalWrite(ledblu, HIGH);
268  //burnout();
269   //abortsystem();
270  sdwrite();
271  rotationmatrices();
272 }
273}
274void datadump   () {
275  if (state >= 1) { 
276//Serial.print("Pitch:  ");
277//Serial.println(Ax);
278//Serial.print("   Roll:  ");
279//Serial.print(Ay);
280//Serial.print(" Yaw:  ");
281//Serial.print(mpu6050.getGyroAngleZ());
282//Serial.println(mpu6050.getAccZ());
283Serial.println(Ay);
284   }
285Serial.print(" System State:  ");
286Serial.println(state);
287
288}
289
290void   sdstart () { 
291if (!SD.begin(chipSelect)) {
292    Serial.println("Card failed,   or not present");
293    // don't do anything more:
294    return;
295  }
296  Serial.println("card   initialized.");
297  
298}
299
300void sdwrite () {
301   String datastring = "";
302   
303 datastring += "Pitch,"; 
304 datastring += String(Ax);
305 datastring +=   ",";
306
307 datastring += "Yaw,";
308 datastring += String(Ay);
309 datastring   += ",";
310 
311 datastring += "Roll,";
312 datastring += String(GyroAngleZ);
313   datastring += ",";
314 
315 datastring += "System_State,";
316 datastring +=   String(state);
317 datastring += ",";
318
319 datastring += "X_Axis_TVC_Angle,";
320   datastring += String(servoX.read() / servoX_gear_ratio);
321 datastring += ",";
322
323   datastring += "Y_Axis_TVC_Angle,";
324 datastring += String(servoY.read() / servoY_gear_ratio);
325   datastring += ",";
326 
327 datastring += "Z_Axis_Accel,";
328 datastring +=   String(accel.getAccelZ_mss());
329 datastring += ",";
330
331 datastring += "Time,";
332   datastring += String(millis() - 10000);
333 datastring += ",";
334
335 datastring   += "TVC_X_int,";
336 datastring += String(pidX_i);
337 datastring += ",";
338
339   datastring += "TVC_Y_int,";
340 datastring += String(pidY_i);
341 datastring +=   ",";
342 
343 datastring += "Altitude,";
344 datastring += String(altitude);
345   datastring += ",";
346
347 datastring += "IMU_Temp,";
348 datastring += String(accel.getTemperature_C());
349   
350 
351  File dataFile = SD.open("f8.txt", FILE_WRITE);
352  
353  if (dataFile)   {
354    dataFile.println(datastring);
355    dataFile.close();
356   
357  }
358   }
359
360
361
362void burnout () { 
363if (state == 1 && accel.getAccelX_mss()   <= 2) {
364  state++;
365  digitalWrite(teensyled, LOW);
366  digitalWrite(ledred,   HIGH);
367  tone(buzzer, 1200, 200);
368  Serial.println("Burnout Detected");
369   delay(1000);
370  recov();
371  
372}
373}
374
375void recov () {
376digitalWrite(pyro1,   HIGH);
377}
378
379void launchpoll () {
380  state == 0;
381  delay(750);
382  Serial.println("Omega   is Armed");
383     int status;
384  status = accel.begin();
385  if (status <   0) {
386    Serial.println("Accel Initialization Error");
387    while (1) {}
388   }
389  status = gyro.begin();
390  if (status < 0) {
391    Serial.println("Gyro   Initialization Error"); 
392    while (1) {}
393  }
394 
395 /* float totalAccelVec   = sqrt(sq(accel.getAccelZ_mss()) + sq(accel.getAccelY_mss()) + sq(accel.getAccelX_mss()));
396   Ax = -asin(accel.getAccelZ_mss() / totalAccelVec);
397  Ay = asin(accel.getAccelY_mss()   / totalAccelVec);*/
398
399
400  delay(500);
401  digitalWrite(ledgrn, HIGH);
402   digitalWrite(ledred, HIGH);
403  digitalWrite(ledblu, LOW);
404  
405}
406void   abortsystem () {
407  if (state == 1 && (Ax > 35 || Ax < -35) || (Ay > 35 || Ay   < -35)){
408    Serial.println("Abort Detected");
409    digitalWrite(ledblu,   LOW);
410    digitalWrite(ledgrn, LOW);
411    digitalWrite(ledred, HIGH);
412    digitalWrite(teensyled,   LOW);
413    tone(buzzer, 1200, 400);
414    state++;
415    state++;
416    recov();
417   }
418}
419

/* Delta Space Systems
      Version 2.31
    September, 10th 2020   */

/* Tuning Guide - README
 *  To tune the servo offsets for your TVC   Mount go to line 52. 
    To tune just raise the values until the servo switches   directions then home in on the perfect value.
 *  To change the ratio between   the Servo and the TVC mount go to line 64. 
    Use a seperate sketch and raise   the degree the servo moves until it hits the end of the TVC Mount. 
    Then   divide the servo degree by 5.
 *  For PID Value Changes go to line 112 - 114.
   *  To change what I/O pins the servos are connected to go to line 139 - 140.  
   */


/*System State: 
 * 0 = Go/No Go before launch
 * 1 = PID Controlled   Ascent
 * 2 = MECO
 * 3 = Abort
 */

//Libraries
#include <SD.h>
#include   <SPI.h>
#include <Servo.h>
#include <Wire.h>
#include <MPU6050_tockn.h>
#include   <math.h>
#include "BMI088.h"
#include <BMP280_DEV.h>                           

/*   accel object */
Bmi088Accel accel(Wire,0x18);
/* gyro object */
Bmi088Gyro   gyro(Wire,0x68);

float temperature, pressure, altitude;            
BMP280_DEV   bmp280;     

double PIDX, PIDY, errorX, errorY, previous_errorX, previous_errorY,   pwmX, pwmY, previouslog, OutX, OutY, OutZ, OreX, OreY, OreZ;
double PreviousGyroX,   PreviousGyroY, PreviousGyroZ, IntGyroX, IntGyroY, RADGyroX, RADGyroY, RADGyroZ,   RawGyZ, DifferenceGyroX, DifferenceGyroY, DifferenceGyroZ, matrix1, matrix2, matrix3;
double   matrix4, matrix5, matrix6, matrix7, matrix8, matrix9, PrevGyX, PrevGyY, PrevGyZ,   RawGyX, RawGyY, GyroAngleX, GyroAngleY, GyroAngleZ, GyroRawX, GyroRawY, GyroRawZ;

//Upright   Angle of the Flight Computer
int desired_angleX = 0;//servoY
int desired_angleY   = 0;//servoX

//Offsets for tuning 
int servoY_offset = 115;
int servoX_offset   = 162;

//Position of servos through the startup function
int servoXstart   = servoY_offset;
int servoYstart = servoX_offset;

//The amount the servo   moves by in the startup function
int servo_start_offset = 20;

//Ratio   between servo gear and tvc mount
float servoX_gear_ratio = 8;
float servoY_gear_ratio   = 8;

double OrientationX = 0;
double OrientationY = 0;
double OrientationZ   = 1;
double Ax;
double Ay;

int ledred = 2;    // LED connected to digital   pin 9
int ledblu = 5;    // LED connected to digital pin 9
int ledgrn = 6;     // LED connected to digital pin 9
int pyro1 = 24;
int buzzer = 21;
int   teensyled = 13;

Servo servoX;
Servo servoY;

double dt, currentTime,   previousTime;

//SD CARD CS
const int chipSelect = BUILTIN_SDCARD;

//"P"   Constants
float pidX_p = 0;
float pidY_p = 0;

//"I" Constants
float   pidY_i = 0;
float pidX_i = 0;

//"D" Constants
float pidX_d = 0;
float   pidY_d = 0;


//PID Gains
double kp = 0.11;
double ki = 0.04;
double   kd = 0.025;

int state;
 
void setup(){
 
  Serial.begin(9600);
   Wire.begin();
  servoX.attach(29);
  servoY.attach(30);
  bmp280.begin(BMP280_I2C_ALT_ADDR);               
  bmp280.startNormalConversion();  
 
  pinMode(ledblu,   OUTPUT);
  pinMode(ledgrn, OUTPUT);
  pinMode(ledred, OUTPUT);
  pinMode(buzzer,   OUTPUT);
  pinMode(pyro1, OUTPUT);
  pinMode(teensyled, OUTPUT);

  startup();
   sdstart();
  launchpoll();
  
}
void loop() {
  //Defining Time   Variables      
  currentTime = millis();            
  dt = (currentTime   - previousTime) / 1000; 
  launchdetect();
  datadump();
  previousTime   = currentTime;  
}

void rotationmatrices () {
  
  //Change Variable   so its easier to refrence later on
  GyroRawX = (gyro.getGyroY_rads());
  GyroRawY   = (gyro.getGyroZ_rads());
  GyroRawZ = (gyro.getGyroX_rads());

  //Integrate   over time to get Local Orientation
  GyroAngleX += GyroRawX * dt;
  GyroAngleY   += GyroRawY * dt;
  GyroAngleZ += GyroRawZ * dt;

  PreviousGyroX = RADGyroX;
   PreviousGyroY = RADGyroY;
  PreviousGyroZ = RADGyroZ;
  
  RADGyroX =   GyroAngleX;
  RADGyroY = GyroAngleY;
  RADGyroZ = GyroAngleZ;
  
  DifferenceGyroX   = (RADGyroX - PreviousGyroX);
  DifferenceGyroY = (RADGyroY - PreviousGyroY);
   DifferenceGyroZ = (RADGyroZ - PreviousGyroZ);

  OreX = OrientationX;
   OreY = OrientationY;
  OreZ = OrientationZ;
  
 //X Matrices
  matrix1   = (cos(DifferenceGyroZ) * cos(DifferenceGyroY));
  matrix2 = (((sin(DifferenceGyroZ)   * -1) * cos(DifferenceGyroX) + (cos(DifferenceGyroZ)) * sin(DifferenceGyroY) * sin(DifferenceGyroX)));
   matrix3 = ((sin(DifferenceGyroZ) * sin(DifferenceGyroX) + (cos(DifferenceGyroZ))   * sin(DifferenceGyroY) * cos(DifferenceGyroX)));
  
 //Y Matrices
  matrix4   = sin(DifferenceGyroZ) * cos(DifferenceGyroY);
  matrix5 = ((cos(DifferenceGyroZ)   * cos(DifferenceGyroX) + (sin(DifferenceGyroZ)) * sin(DifferenceGyroY) * sin(DifferenceGyroX)));
   matrix6 = (((cos(DifferenceGyroZ) * -1) * sin(DifferenceGyroX) + (sin(DifferenceGyroZ))   * sin(DifferenceGyroY) * cos(DifferenceGyroX)));

 //Z Matrices
  matrix7   = (sin(DifferenceGyroY)) * -1;
  matrix8 = cos(DifferenceGyroY) * sin(DifferenceGyroX);
   matrix9 = cos(DifferenceGyroY) * cos(DifferenceGyroX);

 
 OrientationX   = ((OreX * matrix1)) + ((OreY * matrix2)) + ((OreZ * matrix3));
 OrientationY   = ((OreX * matrix4)) + ((OreY * matrix5)) + ((OreZ * matrix6));
 OrientationZ   = ((OreX * matrix7)) + ((OreY * matrix8)) + ((OreZ * matrix9));

Ax = asin(OrientationX)   * (-180 / PI);
Ay = asin(OrientationY) * (180 / PI);
pidcompute();

}

void   pidcompute () {
previous_errorX = errorX;
previous_errorY = errorY; 

errorX   = Ax - desired_angleX;
errorY = Ay - desired_angleY;

//Defining "P"   
pidX_p = kp * errorX;
pidY_p = kp * errorY;

//Defining "D"
pidX_d   = kd*((errorX - previous_errorX)/dt);
pidY_d = kd*((errorY - previous_errorY)/dt);

//Defining   "I"
pidX_i = ki * (pidX_i + errorX * dt);
pidY_i = ki * (pidY_i + errorY   * dt);

//Adding it all up
PIDX = pidX_p + pidX_i + pidX_d;
PIDY = pidY_p   + pidY_i + pidY_d;
 
pwmY = ((PIDY * servoY_gear_ratio) + servoX_offset);
pwmX   = ((PIDX * servoX_gear_ratio) + servoY_offset); 

 
//Servo outputs
servoX.write(pwmX);
servoY.write(pwmY);


}

void   startup () {
  tone(buzzer, 1050, 800);
  digitalWrite(ledblu, HIGH);
   servoX.write(servoXstart);
  servoY.write(servoYstart);
  delay(500);
   digitalWrite(ledblu, LOW);
  digitalWrite(ledgrn, HIGH);
  servoX.write(servoXstart   + servo_start_offset);
  delay(200);
  servoY.write(servoYstart + servo_start_offset);
   digitalWrite(teensyled, HIGH);
  delay(200);
  digitalWrite(ledgrn, LOW);
   digitalWrite(ledred, HIGH);
  servoX.write(servoXstart);
  delay(200);
   servoY.write(servoYstart);
  delay(200);
  digitalWrite(ledred, LOW);
   digitalWrite(ledblu, HIGH);
  tone(buzzer, 1100, 300);
  servoX.write(servoXstart   - servo_start_offset);
  delay(200);
  tone(buzzer, 1150, 250);
  servoY.write(servoYstart   - servo_start_offset);
  delay(200);
  tone(buzzer, 1200, 200);
  servoX.write(servoXstart);
   delay(200);
  servoY.write(servoYstart);
  delay(500);
  
  
 }
void   launchdetect () {
  
  accel.readSensor();
  if (state == 0 && accel.getAccelX_mss()   > 13) {
  state++;
  }
  if (state == 1) {
  gyro.readSensor();
   digitalWrite(ledred, LOW);
  digitalWrite(ledblu, HIGH);
  //burnout();
   //abortsystem();
  sdwrite();
  rotationmatrices();
 }
}
void datadump   () {
  if (state >= 1) { 
//Serial.print("Pitch:  ");
//Serial.println(Ax);
//Serial.print("   Roll:  ");
//Serial.print(Ay);
//Serial.print(" Yaw:  ");
//Serial.print(mpu6050.getGyroAngleZ());
//Serial.println(mpu6050.getAccZ());
Serial.println(Ay);
   }
Serial.print(" System State:  ");
Serial.println(state);

}

void   sdstart () { 
if (!SD.begin(chipSelect)) {
    Serial.println("Card failed,   or not present");
    // don't do anything more:
    return;
  }
  Serial.println("card   initialized.");
  
}

void sdwrite () {
   String datastring = "";
   
 datastring += "Pitch,"; 
 datastring += String(Ax);
 datastring +=   ",";

 datastring += "Yaw,";
 datastring += String(Ay);
 datastring   += ",";
 
 datastring += "Roll,";
 datastring += String(GyroAngleZ);
   datastring += ",";
 
 datastring += "System_State,";
 datastring +=   String(state);
 datastring += ",";

 datastring += "X_Axis_TVC_Angle,";
   datastring += String(servoX.read() / servoX_gear_ratio);
 datastring += ",";

   datastring += "Y_Axis_TVC_Angle,";
 datastring += String(servoY.read() / servoY_gear_ratio);
   datastring += ",";
 
 datastring += "Z_Axis_Accel,";
 datastring +=   String(accel.getAccelZ_mss());
 datastring += ",";

 datastring += "Time,";
   datastring += String(millis() - 10000);
 datastring += ",";

 datastring   += "TVC_X_int,";
 datastring += String(pidX_i);
 datastring += ",";

   datastring += "TVC_Y_int,";
 datastring += String(pidY_i);
 datastring +=   ",";
 
 datastring += "Altitude,";
 datastring += String(altitude);
   datastring += ",";

 datastring += "IMU_Temp,";
 datastring += String(accel.getTemperature_C());
   
 
  File dataFile = SD.open("f8.txt", FILE_WRITE);
  
  if (dataFile)   {
    dataFile.println(datastring);
    dataFile.close();
   
  }
   }



void burnout () { 
if (state == 1 && accel.getAccelX_mss()   <= 2) {
  state++;
  digitalWrite(teensyled, LOW);
  digitalWrite(ledred,   HIGH);
  tone(buzzer, 1200, 200);
  Serial.println("Burnout Detected");
   delay(1000);
  recov();
  
}
}

void recov () {
digitalWrite(pyro1,   HIGH);
}

void launchpoll () {
  state == 0;
  delay(750);
  Serial.println("Omega   is Armed");
     int status;
  status = accel.begin();
  if (status <   0) {
    Serial.println("Accel Initialization Error");
    while (1) {}
   }
  status = gyro.begin();
  if (status < 0) {
    Serial.println("Gyro   Initialization Error"); 
    while (1) {}
  }
 
 /* float totalAccelVec   = sqrt(sq(accel.getAccelZ_mss()) + sq(accel.getAccelY_mss()) + sq(accel.getAccelX_mss()));
   Ax = -asin(accel.getAccelZ_mss() / totalAccelVec);
  Ay = asin(accel.getAccelY_mss()   / totalAccelVec);*/


  delay(500);
  digitalWrite(ledgrn, HIGH);
   digitalWrite(ledred, HIGH);
  digitalWrite(ledblu, LOW);
  
}
void   abortsystem () {
  if (state == 1 && (Ax > 35 || Ax < -35) || (Ay > 35 || Ay   < -35)){
    Serial.println("Abort Detected");
    digitalWrite(ledblu,   LOW);
    digitalWrite(ledgrn, LOW);
    digitalWrite(ledred, HIGH);
    digitalWrite(teensyled,   LOW);
    tone(buzzer, 1200, 400);
    state++;
    state++;
    recov();
   }
}

OmegaSoft-2.38.ino

c_cpp

1/* Delta Space Systems
2      Version 2.31
3    September, 10th 2020  */
4
5/* Tuning Guide - README
6 *  To tune the servo offsets for your TVC  Mount go to line 52. 
7    To tune just raise the values until the servo switches  directions then home in on the perfect value.
8 *  To change the ratio between  the Servo and the TVC mount go to line 64. 
9    Use a seperate sketch and raise  the degree the servo moves until it hits the end of the TVC Mount. 
10    Then  divide the servo degree by 5.
11 *  For PID Value Changes go to line 112 - 114.
12  *  To change what I/O pins the servos are connected to go to line 139 - 140.  
13  */
14
15
16/*System State: 
17 * 0 = Go/No Go before launch
18 * 1 = PID Controlled  Ascent
19 * 2 = MECO
20 * 3 = Abort
21 */
22
23//Libraries
24#include <SD.h>
25#include  <SPI.h>
26#include <Servo.h>
27#include <Wire.h>
28#include <MPU6050_tockn.h>
29#include  <math.h>
30#include "BMI088.h"
31#include <BMP280_DEV.h>                           
32
33/*  accel object */
34Bmi088Accel accel(Wire,0x18);
35/* gyro object */
36Bmi088Gyro  gyro(Wire,0x68);
37
38float temperature, pressure, altitude;            
39BMP280_DEV  bmp280;     
40
41double PIDX, PIDY, errorX, errorY, previous_errorX, previous_errorY,  pwmX, pwmY, previouslog, OutX, OutY, OutZ, OreX, OreY, OreZ;
42double PreviousGyroX,  PreviousGyroY, PreviousGyroZ, IntGyroX, IntGyroY, RADGyroX, RADGyroY, RADGyroZ,  RawGyZ, DifferenceGyroX, DifferenceGyroY, DifferenceGyroZ, matrix1, matrix2, matrix3;
43double  matrix4, matrix5, matrix6, matrix7, matrix8, matrix9, PrevGyX, PrevGyY, PrevGyZ,  RawGyX, RawGyY, GyroAngleX, GyroAngleY, GyroAngleZ, GyroRawX, GyroRawY, GyroRawZ;
44
45//Upright  Angle of the Flight Computer
46int desired_angleX = 0;//servoY
47int desired_angleY  = 0;//servoX
48
49//Offsets for tuning 
50int servoY_offset = 115;
51int servoX_offset  = 162;
52
53//Position of servos through the startup function
54int servoXstart  = servoY_offset;
55int servoYstart = servoX_offset;
56
57//The amount the servo  moves by in the startup function
58int servo_start_offset = 20;
59
60//Ratio  between servo gear and tvc mount
61float servoX_gear_ratio = 8;
62float servoY_gear_ratio  = 8;
63
64double OrientationX = 0;
65double OrientationY = 0;
66double OrientationZ  = 1;
67double Ax;
68double Ay;
69
70int ledred = 2;    // LED connected to digital  pin 9
71int ledblu = 5;    // LED connected to digital pin 9
72int ledgrn = 6;    // LED connected to digital pin 9
73int pyro1 = 24;
74int buzzer = 21;
75int  teensyled = 13;
76
77Servo servoX;
78Servo servoY;
79
80double dt, currentTime,  previousTime;
81
82//SD CARD CS
83const int chipSelect = BUILTIN_SDCARD;
84
85//"P"  Constants
86float pidX_p = 0;
87float pidY_p = 0;
88
89//"I" Constants
90float  pidY_i = 0;
91float pidX_i = 0;
92
93//"D" Constants
94float pidX_d = 0;
95float  pidY_d = 0;
96
97
98//PID Gains
99double kp = 0.11;
100double ki = 0.04;
101double  kd = 0.025;
102
103int state;
104 
105void setup(){
106 
107  Serial.begin(9600);
108  Wire.begin();
109  servoX.attach(29);
110  servoY.attach(30);
111  bmp280.begin(BMP280_I2C_ALT_ADDR);              
112  bmp280.startNormalConversion();  
113 
114  pinMode(ledblu,  OUTPUT);
115  pinMode(ledgrn, OUTPUT);
116  pinMode(ledred, OUTPUT);
117  pinMode(buzzer,  OUTPUT);
118  pinMode(pyro1, OUTPUT);
119  pinMode(teensyled, OUTPUT);
120
121  startup();
122  sdstart();
123  launchpoll();
124  
125}
126void loop() {
127  //Defining Time  Variables      
128  currentTime = millis();            
129  dt = (currentTime  - previousTime) / 1000; 
130  launchdetect();
131  datadump();
132  previousTime  = currentTime;  
133}
134
135void rotationmatrices () {
136  
137  //Change Variable  so its easier to refrence later on
138  GyroRawX = (gyro.getGyroY_rads());
139  GyroRawY  = (gyro.getGyroZ_rads());
140  GyroRawZ = (gyro.getGyroX_rads());
141
142  //Integrate  over time to get Local Orientation
143  GyroAngleX += GyroRawX * dt;
144  GyroAngleY  += GyroRawY * dt;
145  GyroAngleZ += GyroRawZ * dt;
146
147  PreviousGyroX = RADGyroX;
148  PreviousGyroY = RADGyroY;
149  PreviousGyroZ = RADGyroZ;
150  
151  RADGyroX =  GyroAngleX;
152  RADGyroY = GyroAngleY;
153  RADGyroZ = GyroAngleZ;
154  
155  DifferenceGyroX  = (RADGyroX - PreviousGyroX);
156  DifferenceGyroY = (RADGyroY - PreviousGyroY);
157  DifferenceGyroZ = (RADGyroZ - PreviousGyroZ);
158
159  OreX = OrientationX;
160  OreY = OrientationY;
161  OreZ = OrientationZ;
162  
163 //X Matrices
164  matrix1  = (cos(DifferenceGyroZ) * cos(DifferenceGyroY));
165  matrix2 = (((sin(DifferenceGyroZ)  * -1) * cos(DifferenceGyroX) + (cos(DifferenceGyroZ)) * sin(DifferenceGyroY) * sin(DifferenceGyroX)));
166  matrix3 = ((sin(DifferenceGyroZ) * sin(DifferenceGyroX) + (cos(DifferenceGyroZ))  * sin(DifferenceGyroY) * cos(DifferenceGyroX)));
167  
168 //Y Matrices
169  matrix4  = sin(DifferenceGyroZ) * cos(DifferenceGyroY);
170  matrix5 = ((cos(DifferenceGyroZ)  * cos(DifferenceGyroX) + (sin(DifferenceGyroZ)) * sin(DifferenceGyroY) * sin(DifferenceGyroX)));
171  matrix6 = (((cos(DifferenceGyroZ) * -1) * sin(DifferenceGyroX) + (sin(DifferenceGyroZ))  * sin(DifferenceGyroY) * cos(DifferenceGyroX)));
172
173 //Z Matrices
174  matrix7  = (sin(DifferenceGyroY)) * -1;
175  matrix8 = cos(DifferenceGyroY) * sin(DifferenceGyroX);
176  matrix9 = cos(DifferenceGyroY) * cos(DifferenceGyroX);
177
178 
179 OrientationX  = ((OreX * matrix1)) + ((OreY * matrix2)) + ((OreZ * matrix3));
180 OrientationY  = ((OreX * matrix4)) + ((OreY * matrix5)) + ((OreZ * matrix6));
181 OrientationZ  = ((OreX * matrix7)) + ((OreY * matrix8)) + ((OreZ * matrix9));
182
183Ax = asin(OrientationX)  * (-180 / PI);
184Ay = asin(OrientationY) * (180 / PI);
185pidcompute();
186
187}
188
189void  pidcompute () {
190previous_errorX = errorX;
191previous_errorY = errorY; 
192
193errorX  = Ax - desired_angleX;
194errorY = Ay - desired_angleY;
195
196//Defining "P"  
197pidX_p = kp * errorX;
198pidY_p = kp * errorY;
199
200//Defining "D"
201pidX_d  = kd*((errorX - previous_errorX)/dt);
202pidY_d = kd*((errorY - previous_errorY)/dt);
203
204//Defining  "I"
205pidX_i = ki * (pidX_i + errorX * dt);
206pidY_i = ki * (pidY_i + errorY  * dt);
207
208//Adding it all up
209PIDX = pidX_p + pidX_i + pidX_d;
210PIDY = pidY_p  + pidY_i + pidY_d;
211 
212pwmY = ((PIDY * servoY_gear_ratio) + servoX_offset);
213pwmX  = ((PIDX * servoX_gear_ratio) + servoY_offset); 
214
215 
216//Servo outputs
217servoX.write(pwmX);
218servoY.write(pwmY);
219
220
221}
222
223void  startup () {
224  tone(buzzer, 1050, 800);
225  digitalWrite(ledblu, HIGH);
226  servoX.write(servoXstart);
227  servoY.write(servoYstart);
228  delay(500);
229  digitalWrite(ledblu, LOW);
230  digitalWrite(ledgrn, HIGH);
231  servoX.write(servoXstart  + servo_start_offset);
232  delay(200);
233  servoY.write(servoYstart + servo_start_offset);
234  digitalWrite(teensyled, HIGH);
235  delay(200);
236  digitalWrite(ledgrn, LOW);
237  digitalWrite(ledred, HIGH);
238  servoX.write(servoXstart);
239  delay(200);
240  servoY.write(servoYstart);
241  delay(200);
242  digitalWrite(ledred, LOW);
243  digitalWrite(ledblu, HIGH);
244  tone(buzzer, 1100, 300);
245  servoX.write(servoXstart  - servo_start_offset);
246  delay(200);
247  tone(buzzer, 1150, 250);
248  servoY.write(servoYstart  - servo_start_offset);
249  delay(200);
250  tone(buzzer, 1200, 200);
251  servoX.write(servoXstart);
252  delay(200);
253  servoY.write(servoYstart);
254  delay(500);
255  
256  
257 }
258void  launchdetect () {
259  
260  accel.readSensor();
261  if (state == 0 && accel.getAccelX_mss()  > 13) {
262  state++;
263  }
264  if (state == 1) {
265  gyro.readSensor();
266  digitalWrite(ledred, LOW);
267  digitalWrite(ledblu, HIGH);
268  //burnout();
269  //abortsystem();
270  sdwrite();
271  rotationmatrices();
272 }
273}
274void datadump  () {
275  if (state >= 1) { 
276//Serial.print("Pitch:  ");
277//Serial.println(Ax);
278//Serial.print("  Roll:  ");
279//Serial.print(Ay);
280//Serial.print(" Yaw:  ");
281//Serial.print(mpu6050.getGyroAngleZ());
282//Serial.println(mpu6050.getAccZ());
283Serial.println(Ay);
284  }
285Serial.print(" System State:  ");
286Serial.println(state);
287
288}
289
290void  sdstart () { 
291if (!SD.begin(chipSelect)) {
292    Serial.println("Card failed,  or not present");
293    // don't do anything more:
294    return;
295  }
296  Serial.println("card  initialized.");
297  
298}
299
300void sdwrite () {
301   String datastring = "";
302  
303 datastring += "Pitch,"; 
304 datastring += String(Ax);
305 datastring +=  ",";
306
307 datastring += "Yaw,";
308 datastring += String(Ay);
309 datastring  += ",";
310 
311 datastring += "Roll,";
312 datastring += String(GyroAngleZ);
313  datastring += ",";
314 
315 datastring += "System_State,";
316 datastring +=  String(state);
317 datastring += ",";
318
319 datastring += "X_Axis_TVC_Angle,";
320  datastring += String(servoX.read() / servoX_gear_ratio);
321 datastring += ",";
322
323  datastring += "Y_Axis_TVC_Angle,";
324 datastring += String(servoY.read() / servoY_gear_ratio);
325  datastring += ",";
326 
327 datastring += "Z_Axis_Accel,";
328 datastring +=  String(accel.getAccelZ_mss());
329 datastring += ",";
330
331 datastring += "Time,";
332  datastring += String(millis() - 10000);
333 datastring += ",";
334
335 datastring  += "TVC_X_int,";
336 datastring += String(pidX_i);
337 datastring += ",";
338
339  datastring += "TVC_Y_int,";
340 datastring += String(pidY_i);
341 datastring +=  ",";
342 
343 datastring += "Altitude,";
344 datastring += String(altitude);
345  datastring += ",";
346
347 datastring += "IMU_Temp,";
348 datastring += String(accel.getTemperature_C());
349  
350 
351  File dataFile = SD.open("f8.txt", FILE_WRITE);
352  
353  if (dataFile)  {
354    dataFile.println(datastring);
355    dataFile.close();
356   
357  }
358  }
359
360
361
362void burnout () { 
363if (state == 1 && accel.getAccelX_mss()  <= 2) {
364  state++;
365  digitalWrite(teensyled, LOW);
366  digitalWrite(ledred,  HIGH);
367  tone(buzzer, 1200, 200);
368  Serial.println("Burnout Detected");
369  delay(1000);
370  recov();
371  
372}
373}
374
375void recov () {
376digitalWrite(pyro1,  HIGH);
377}
378
379void launchpoll () {
380  state == 0;
381  delay(750);
382  Serial.println("Omega  is Armed");
383     int status;
384  status = accel.begin();
385  if (status <  0) {
386    Serial.println("Accel Initialization Error");
387    while (1) {}
388  }
389  status = gyro.begin();
390  if (status < 0) {
391    Serial.println("Gyro  Initialization Error"); 
392    while (1) {}
393  }
394 
395 /* float totalAccelVec  = sqrt(sq(accel.getAccelZ_mss()) + sq(accel.getAccelY_mss()) + sq(accel.getAccelX_mss()));
396  Ax = -asin(accel.getAccelZ_mss() / totalAccelVec);
397  Ay = asin(accel.getAccelY_mss()  / totalAccelVec);*/
398
399
400  delay(500);
401  digitalWrite(ledgrn, HIGH);
402  digitalWrite(ledred, HIGH);
403  digitalWrite(ledblu, LOW);
404  
405}
406void  abortsystem () {
407  if (state == 1 && (Ax > 35 || Ax < -35) || (Ay > 35 || Ay  < -35)){
408    Serial.println("Abort Detected");
409    digitalWrite(ledblu,  LOW);
410    digitalWrite(ledgrn, LOW);
411    digitalWrite(ledred, HIGH);
412    digitalWrite(teensyled,  LOW);
413    tone(buzzer, 1200, 400);
414    state++;
415    state++;
416    recov();
417  }
418}
419

/* Delta Space Systems
      Version 2.31
    September, 10th 2020  */

/* Tuning Guide - README
 *  To tune the servo offsets for your TVC  Mount go to line 52. 
    To tune just raise the values until the servo switches  directions then home in on the perfect value.
 *  To change the ratio between  the Servo and the TVC mount go to line 64. 
    Use a seperate sketch and raise  the degree the servo moves until it hits the end of the TVC Mount. 
    Then  divide the servo degree by 5.
 *  For PID Value Changes go to line 112 - 114.
  *  To change what I/O pins the servos are connected to go to line 139 - 140.  
  */


/*System State: 
 * 0 = Go/No Go before launch
 * 1 = PID Controlled  Ascent
 * 2 = MECO
 * 3 = Abort
 */

//Libraries
#include <SD.h>
#include  <SPI.h>
#include <Servo.h>
#include <Wire.h>
#include <MPU6050_tockn.h>
#include  <math.h>
#include "BMI088.h"
#include <BMP280_DEV.h>                           

/*  accel object */
Bmi088Accel accel(Wire,0x18);
/* gyro object */
Bmi088Gyro  gyro(Wire,0x68);

float temperature, pressure, altitude;            
BMP280_DEV  bmp280;     

double PIDX, PIDY, errorX, errorY, previous_errorX, previous_errorY,  pwmX, pwmY, previouslog, OutX, OutY, OutZ, OreX, OreY, OreZ;
double PreviousGyroX,  PreviousGyroY, PreviousGyroZ, IntGyroX, IntGyroY, RADGyroX, RADGyroY, RADGyroZ,  RawGyZ, DifferenceGyroX, DifferenceGyroY, DifferenceGyroZ, matrix1, matrix2, matrix3;
double  matrix4, matrix5, matrix6, matrix7, matrix8, matrix9, PrevGyX, PrevGyY, PrevGyZ,  RawGyX, RawGyY, GyroAngleX, GyroAngleY, GyroAngleZ, GyroRawX, GyroRawY, GyroRawZ;

//Upright  Angle of the Flight Computer
int desired_angleX = 0;//servoY
int desired_angleY  = 0;//servoX

//Offsets for tuning 
int servoY_offset = 115;
int servoX_offset  = 162;

//Position of servos through the startup function
int servoXstart  = servoY_offset;
int servoYstart = servoX_offset;

//The amount the servo  moves by in the startup function
int servo_start_offset = 20;

//Ratio  between servo gear and tvc mount
float servoX_gear_ratio = 8;
float servoY_gear_ratio  = 8;

double OrientationX = 0;
double OrientationY = 0;
double OrientationZ  = 1;
double Ax;
double Ay;

int ledred = 2;    // LED connected to digital  pin 9
int ledblu = 5;    // LED connected to digital pin 9
int ledgrn = 6;    // LED connected to digital pin 9
int pyro1 = 24;
int buzzer = 21;
int  teensyled = 13;

Servo servoX;
Servo servoY;

double dt, currentTime,  previousTime;

//SD CARD CS
const int chipSelect = BUILTIN_SDCARD;

//"P"  Constants
float pidX_p = 0;
float pidY_p = 0;

//"I" Constants
float  pidY_i = 0;
float pidX_i = 0;

//"D" Constants
float pidX_d = 0;
float  pidY_d = 0;


//PID Gains
double kp = 0.11;
double ki = 0.04;
double  kd = 0.025;

int state;
 
void setup(){
 
  Serial.begin(9600);
  Wire.begin();
  servoX.attach(29);
  servoY.attach(30);
  bmp280.begin(BMP280_I2C_ALT_ADDR);              
  bmp280.startNormalConversion();  
 
  pinMode(ledblu,  OUTPUT);
  pinMode(ledgrn, OUTPUT);
  pinMode(ledred, OUTPUT);
  pinMode(buzzer,  OUTPUT);
  pinMode(pyro1, OUTPUT);
  pinMode(teensyled, OUTPUT);

  startup();
  sdstart();
  launchpoll();
  
}
void loop() {
  //Defining Time  Variables      
  currentTime = millis();            
  dt = (currentTime  - previousTime) / 1000; 
  launchdetect();
  datadump();
  previousTime  = currentTime;  
}

void rotationmatrices () {
  
  //Change Variable  so its easier to refrence later on
  GyroRawX = (gyro.getGyroY_rads());
  GyroRawY  = (gyro.getGyroZ_rads());
  GyroRawZ = (gyro.getGyroX_rads());

  //Integrate  over time to get Local Orientation
  GyroAngleX += GyroRawX * dt;
  GyroAngleY  += GyroRawY * dt;
  GyroAngleZ += GyroRawZ * dt;

  PreviousGyroX = RADGyroX;
  PreviousGyroY = RADGyroY;
  PreviousGyroZ = RADGyroZ;
  
  RADGyroX =  GyroAngleX;
  RADGyroY = GyroAngleY;
  RADGyroZ = GyroAngleZ;
  
  DifferenceGyroX  = (RADGyroX - PreviousGyroX);
  DifferenceGyroY = (RADGyroY - PreviousGyroY);
  DifferenceGyroZ = (RADGyroZ - PreviousGyroZ);

  OreX = OrientationX;
  OreY = OrientationY;
  OreZ = OrientationZ;
  
 //X Matrices
  matrix1  = (cos(DifferenceGyroZ) * cos(DifferenceGyroY));
  matrix2 = (((sin(DifferenceGyroZ)  * -1) * cos(DifferenceGyroX) + (cos(DifferenceGyroZ)) * sin(DifferenceGyroY) * sin(DifferenceGyroX)));
  matrix3 = ((sin(DifferenceGyroZ) * sin(DifferenceGyroX) + (cos(DifferenceGyroZ))  * sin(DifferenceGyroY) * cos(DifferenceGyroX)));
  
 //Y Matrices
  matrix4  = sin(DifferenceGyroZ) * cos(DifferenceGyroY);
  matrix5 = ((cos(DifferenceGyroZ)  * cos(DifferenceGyroX) + (sin(DifferenceGyroZ)) * sin(DifferenceGyroY) * sin(DifferenceGyroX)));
  matrix6 = (((cos(DifferenceGyroZ) * -1) * sin(DifferenceGyroX) + (sin(DifferenceGyroZ))  * sin(DifferenceGyroY) * cos(DifferenceGyroX)));

 //Z Matrices
  matrix7  = (sin(DifferenceGyroY)) * -1;
  matrix8 = cos(DifferenceGyroY) * sin(DifferenceGyroX);
  matrix9 = cos(DifferenceGyroY) * cos(DifferenceGyroX);

 
 OrientationX  = ((OreX * matrix1)) + ((OreY * matrix2)) + ((OreZ * matrix3));
 OrientationY  = ((OreX * matrix4)) + ((OreY * matrix5)) + ((OreZ * matrix6));
 OrientationZ  = ((OreX * matrix7)) + ((OreY * matrix8)) + ((OreZ * matrix9));

Ax = asin(OrientationX)  * (-180 / PI);
Ay = asin(OrientationY) * (180 / PI);
pidcompute();

}

void  pidcompute () {
previous_errorX = errorX;
previous_errorY = errorY; 

errorX  = Ax - desired_angleX;
errorY = Ay - desired_angleY;

//Defining "P"  
pidX_p = kp * errorX;
pidY_p = kp * errorY;

//Defining "D"
pidX_d  = kd*((errorX - previous_errorX)/dt);
pidY_d = kd*((errorY - previous_errorY)/dt);

//Defining  "I"
pidX_i = ki * (pidX_i + errorX * dt);
pidY_i = ki * (pidY_i + errorY  * dt);

//Adding it all up
PIDX = pidX_p + pidX_i + pidX_d;
PIDY = pidY_p  + pidY_i + pidY_d;
 
pwmY = ((PIDY * servoY_gear_ratio) + servoX_offset);
pwmX  = ((PIDX * servoX_gear_ratio) + servoY_offset); 

 
//Servo outputs
servoX.write(pwmX);
servoY.write(pwmY);


}

void  startup () {
  tone(buzzer, 1050, 800);
  digitalWrite(ledblu, HIGH);
  servoX.write(servoXstart);
  servoY.write(servoYstart);
  delay(500);
  digitalWrite(ledblu, LOW);
  digitalWrite(ledgrn, HIGH);
  servoX.write(servoXstart  + servo_start_offset);
  delay(200);
  servoY.write(servoYstart + servo_start_offset);
  digitalWrite(teensyled, HIGH);
  delay(200);
  digitalWrite(ledgrn, LOW);
  digitalWrite(ledred, HIGH);
  servoX.write(servoXstart);
  delay(200);
  servoY.write(servoYstart);
  delay(200);
  digitalWrite(ledred, LOW);
  digitalWrite(ledblu, HIGH);
  tone(buzzer, 1100, 300);
  servoX.write(servoXstart  - servo_start_offset);
  delay(200);
  tone(buzzer, 1150, 250);
  servoY.write(servoYstart  - servo_start_offset);
  delay(200);
  tone(buzzer, 1200, 200);
  servoX.write(servoXstart);
  delay(200);
  servoY.write(servoYstart);
  delay(500);
  
  
 }
void  launchdetect () {
  
  accel.readSensor();
  if (state == 0 && accel.getAccelX_mss()  > 13) {
  state++;
  }
  if (state == 1) {
  gyro.readSensor();
  digitalWrite(ledred, LOW);
  digitalWrite(ledblu, HIGH);
  //burnout();
  //abortsystem();
  sdwrite();
  rotationmatrices();
 }
}
void datadump  () {
  if (state >= 1) { 
//Serial.print("Pitch:  ");
//Serial.println(Ax);
//Serial.print("  Roll:  ");
//Serial.print(Ay);
//Serial.print(" Yaw:  ");
//Serial.print(mpu6050.getGyroAngleZ());
//Serial.println(mpu6050.getAccZ());
Serial.println(Ay);
  }
Serial.print(" System State:  ");
Serial.println(state);

}

void  sdstart () { 
if (!SD.begin(chipSelect)) {
    Serial.println("Card failed,  or not present");
    // don't do anything more:
    return;
  }
  Serial.println("card  initialized.");
  
}

void sdwrite () {
   String datastring = "";
  
 datastring += "Pitch,"; 
 datastring += String(Ax);
 datastring +=  ",";

 datastring += "Yaw,";
 datastring += String(Ay);
 datastring  += ",";
 
 datastring += "Roll,";
 datastring += String(GyroAngleZ);
  datastring += ",";
 
 datastring += "System_State,";
 datastring +=  String(state);
 datastring += ",";

 datastring += "X_Axis_TVC_Angle,";
  datastring += String(servoX.read() / servoX_gear_ratio);
 datastring += ",";

  datastring += "Y_Axis_TVC_Angle,";
 datastring += String(servoY.read() / servoY_gear_ratio);
  datastring += ",";
 
 datastring += "Z_Axis_Accel,";
 datastring +=  String(accel.getAccelZ_mss());
 datastring += ",";

 datastring += "Time,";
  datastring += String(millis() - 10000);
 datastring += ",";

 datastring  += "TVC_X_int,";
 datastring += String(pidX_i);
 datastring += ",";

  datastring += "TVC_Y_int,";
 datastring += String(pidY_i);
 datastring +=  ",";
 
 datastring += "Altitude,";
 datastring += String(altitude);
  datastring += ",";

 datastring += "IMU_Temp,";
 datastring += String(accel.getTemperature_C());
  
 
  File dataFile = SD.open("f8.txt", FILE_WRITE);
  
  if (dataFile)  {
    dataFile.println(datastring);
    dataFile.close();
   
  }
  }



void burnout () { 
if (state == 1 && accel.getAccelX_mss()  <= 2) {
  state++;
  digitalWrite(teensyled, LOW);
  digitalWrite(ledred,  HIGH);
  tone(buzzer, 1200, 200);
  Serial.println("Burnout Detected");
  delay(1000);
  recov();
  
}
}

void recov () {
digitalWrite(pyro1,  HIGH);
}

void launchpoll () {
  state == 0;
  delay(750);
  Serial.println("Omega  is Armed");
     int status;
  status = accel.begin();
  if (status <  0) {
    Serial.println("Accel Initialization Error");
    while (1) {}
  }
  status = gyro.begin();
  if (status < 0) {
    Serial.println("Gyro  Initialization Error"); 
    while (1) {}
  }
 
 /* float totalAccelVec  = sqrt(sq(accel.getAccelZ_mss()) + sq(accel.getAccelY_mss()) + sq(accel.getAccelX_mss()));
  Ax = -asin(accel.getAccelZ_mss() / totalAccelVec);
  Ay = asin(accel.getAccelY_mss()  / totalAccelVec);*/


  delay(500);
  digitalWrite(ledgrn, HIGH);
  digitalWrite(ledred, HIGH);
  digitalWrite(ledblu, LOW);
  
}
void  abortsystem () {
  if (state == 1 && (Ax > 35 || Ax < -35) || (Ay > 35 || Ay  < -35)){
    Serial.println("Abort Detected");
    digitalWrite(ledblu,  LOW);
    digitalWrite(ledgrn, LOW);
    digitalWrite(ledred, HIGH);
    digitalWrite(teensyled,  LOW);
    tone(buzzer, 1200, 400);
    state++;
    state++;
    recov();
  }
}

CODE NOT AVALIABLE

c_cpp

1ODE NOT AVALIABL

ODE NOT AVALIABL

Downloadable files

pcb_mk6_board_2020-09-26_23-34-43_R3mQNcUbWK.pdf

Documentation

tvc_mk_2_outer_gimbal_r6_lsBoLJRUPw.stl

tvc_motor_tube_mk_2_gxhSae4xCl.stl

tvc_mk_2_outer_gimbal_r6_lsBoLJRUPw.stl

tvc_motor_tube_mk_2_gxhSae4xCl.stl

tvc_mk_2_inner_gimbal_r6_4tbFjPC1Cy.stl

Comments

Only logged in users can leave comments

UniverseRobotics

0 Followers

•

0 Projects

Intro